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ABSTRACT 

A  six  degree  of  freedom  robot  manipulator  arm,  a  PUMA  560,  is  calibrated  using  full 
pose  and  partial  pose  methods  in  order  to  improve  the  accuracy  of  the  manipulator  arm. 
The  theory  applicable  to  modeling  of  mechanisms  is  introduced.  A  thirty  parameter 
kinematic  model  is  developed  for  use  in  the  full  pose  calibration  method  and  a  twenty-six 
parameter  kinematic  model  is  developed  for  the  partial  pose  calibration.  A  simulation  study 
is  performed  to  determine  the  applicability  and  feasibility  of  each  model.  Experimental 
pose  measurements  are  performed  using  each  method  to  obtain  data  with  which  to  perform 
an  actual  calibration  of  the  manipulator  and  compare  with  the  predicted  results.  The  effects 
of  noise  in  each  measurement  system  employed  and  in  the  manipulator's  joint  position 
encoders  are  discussed.  The  measurement  systems  employed  are  examined  in  detail  and  a 
comparison  between  the  two  is  performed. 

The  measured  kinematic  parameters  of  each  model  are  presented  as  results. 
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I.       INTRODUCTION 

In  the  calibration  of  a  manipulator  the  desired  result  is  to  significantly  improve  the 
manipulator's  accuracy.  Accuracy  is  defined  as  the  ability  of  a  robot  to  move  to  a 
commanded  pose  defined  in  the  manipulator's  working  volume  [Ref.  1].  Stated  simply. 
how  accurately  can  the  origin  of  a  coordinate  frame  and  the  orientation  of  the  coordinate 
axes  attached  to  the  end  effector  of  the  manipulator  arm,  to  be  termed  achieving  a  certain 
pose,  be  positioned  in  relation  to  a  commanded  point  and  orientation  in  the  manipulator's 
working  volume?  Present  experimentation  shows  that  manipulators  have  an  accuracy  of 
about  10.0  mm  [Ref.  2]. 

Another  indication  of  a  manipulator's  ability  to  achieve  a  pose  is  it's  repeatability. 
Repeatability  is  the  ability  of  a  manipulator  to  return  to  a  previously  achieved  pose  [Ref.  3]. 
To  illustrate  repeatability,  consider  the  following.  A  manipulator  arm's  end  effector, 
termed  the  tool,  is  moved  to  a  certain  position  and  orientation  in  the  working  volume.  After 
the  position  of  each  joint  angle  of  the  manipulator  is  recorded,  the  tool's  position  and 
orientation  is  altered.  The  manipulator  is  commanded  to  assume  the  previously  recorded 
joint  angles.  Examination  will  reveal  that  the  tool's  position  and  orientation  will  differ  from 
the  originally  learned  pose.  This  is  the  error  in  repeatability  of  a  manipulator  arm.  Present 
experimentation  shows  that  manipulators  have  an  repeatability  of  around  0.3  mm  [Ref.  4]. 

To  summarize,  the  fundamental  difference  between  accuracy  and  repeatability  is  that 
repeatability  is  the  ability  of  the  manipulator  to  return  to  a  previously  achieved  pose  and 
accuracy  is  the  ability  of  the  manipulator  to  move  to  a  pose  that  is  specified  in  the  working 
volume  and  that  may  have  never  been  previously  reached.  It  is  apparent  that  a  more 
accurate  manipulator  is  desired  over  a  more  repeatable  manipulator.  The  accurate 
manipulator's  working  volume  is  unconstrained  while  the  repeatable  manipulator  does  not 
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have  a  working  volume,  only  discrete  positions  which  must  be  taught  to  the  manipulator. 
Correspondingly,  the  objective  of  the  calibration  process  is  to  improve  the  accuracy  of  the 
manipulator. 

In  order  to  understand  how  a  calibration  is  performed  on  a  manipulator  one  must 
understand  how  a  commanded  pose  would  be  achieved  by  any  manipulator.  When  a 
specific  position  and  orientation  is  commanded  to  the  manipulator,  it  must  be  specified 
relative  to  a  world  coordinate  frame  defined  by  the  user.  To  the  manipulator  this  defined 
world  coordinate  frame  is,  as  described,  the  center  of  the  universe.  By  means  of  several 
mathematical  operations  the  coordinate  frame  originally  attached  to  the  world  coordinate 
frame  is  moved  from  the  world  coordinate  frame  to  the  manipulator'  base  and  through  each 
arm  of  the  manipulator  until  it  reaches  the  tool.  This  implies  a  knowledge  of  a  mathematical 
model  of  the  manipulator  which  includes  a  world  coordinate  frame. 

As  it  is  prohibitively  expensive  to  manufacture  and  assemble  each  manipulator  to  the 
exact  specifications  required  for  desired  accuracy,  every  manipulator  built  has  a  unique 
geometry.  Since  each  manipulator  has  this  unique  geometry,  each  will  have  a  unique 
mathematical  model.  The  calibration  process  will  identify  the  kinematic  parameters 
embedded  within  the  manipulator  in  order  that  the  individual  mathematical  model  will 
accurately  represent  the  physical  manipulator. 

In  the  calibration  process,  several  sequential  steps  enable  the  precise  kinematic 
parameters  of  the  manipulator  to  be  identified,  leading  to  improved  accuracy.  These  steps 
are  described  as  follows  [Ref.  5]: 

1 .  A  kinematic  model  of  the  manipulator  and  calibration  process  is  developed.  The 
resulting  model  is  used  to  define  a  pose  error  quantity  based  on  a  nominal  kinematic 
parameter  set,  an  unknown  parameter  set  representing  the  true  geometry  of  the  manipulator 
and  a  set  of  joint  angles.  The  nominal  kinematics  would  be  supplied  by  the  manufacturer  of 


the  manipulator,  while  the  unknown,  actual  parameter  set  will   be   identified    in   the 
calibration  process. 

2.  Experimental  measurements  of  the  robot  pose,  which  could  be  either  full  pose 
or  partial  pose,  are  taken  in  order  to  obtain  data  which  correspond  to  the  actual  kinematic 
parameters  of  the  manipulator. 

3.  The  actual  kinematic  parameters  are  identified  by  systematically  changing  the 
nominal  parameter  set  so  as  to  reduce  the  error  quantity  defined  in  the  modeling  phase. 
This  is  performed  as  a  multidimensional  optimization  search  in  which  the  identified 
kinematic  parameters  are  changed  in  order  to  reduce  an  error  function  to  a  minimum 
amount. 

4.  The  final  step  involves  incorporating  the  identified  parameters  into  the  actual 
controlling  software  of  the  manipulator. 

Once  the  calibration  process  is  complete  the  next  step  is  to  implement  the  identified 
kinematic  parameters  into  the  operation  of  the  manipulator  arm.  In  an  operational  setting 
the  required  position  and  orientation  of  the  manipulator  arm's  end  effector  would  be  input 
to  a  controlling  software  program.  This  control  program  would  perform  an  inverse 
kinematic  solution  through  the  mathematical  model  of  the  manipulator  encoded  within  the 
control  scheme,  calculating  the  required  joint  angles  to  position  and  orient  the  end  effector 
as  dictated  by  the  operator. 

This  work  addresses  the  issue  of  developing  the  kinematic  model  which  represents 
the  PUMA  manipulator  arm  and  gathering  the  experimental  data  used  in  the  calibration 
process.  Two  methods  will  be  described  in  this  thesis:  a  full  pose  calibration  process  and  a 
partial  pose  calibration  process. 

The  full  pose  calibration  process  will  provide  the  benchmark  calibration  since  the  full 
pose  of  the  end  effector  will  be  measured  in  each  observation. 


Once  the  benchmark  kinematic  parameters  are  identified  the  second  method  of 
calibration,  the  partial  pose  calibration  will  be  performed.  The  resulting  kinematic  model 
produced  by  this  calibration  will  be  contrasted  with  the  values  identified  in  the  benchmark. 


II.     THEORY 

A.     MODELING 

The  theory  presented  and  several  of  the  diagrams  in  this  chapter  follows  closely  that 
presented  by  Paul  [Ref.  6]  in  Chapters  I  through  III.  Also,  several  diagrams  and 
explanations  in  this  chapter  are  based  on  readings  from  Mooring,  Roth,  and  Driels  [Ref. 

7]- 

1.      Homogeneous  Transformations 

The  most  basic  premise  on  which  the  calibration  process  is  associated  is  the 
movement  of  a  set  of  coordinate  axes  from  one  position  and  orientation  in  space  to  another 
position  and  orientation,  as  illustrated  in  Figure  1.  In  Figure  1.  the  coordinate  axes  at  point 
A  are  transformed,  via  translations  and  rotations,  to  point  B. 


Figure   1.     A  Homogeneous  Transformation 


In  addition  to  the  translation  of  the  origin  of  the  coordinate  axes  from  one 
position  in  space  to  another  there  is  a  change  in  the  orientation  of  the  coordinate  axes.  The 
process  of  the  movement  of  the  coordinate  axes  is  known  as  a  homogeneous 
transformation.  This  homogeneous  transformation  is  accomplished  by  a  series  of 
translational  transformations  and  rotational  transformations. 

The  first  type  of  transformation  is  the  translational  transformation.  Given  a 
point  in  space,  A,  illustrated  in  Figure  2.  the  point's  position  is  fixed  and  known  relative  to 
a  fixed,  three  axis  coordinate  frame,  defined  as  the  world  coordinate  frame,  by  means  of 
the  following  vector,  u: 


u  =    - 

z 

L-W- 


(1) 


Figure  2.     Position  of  Point  A  with  respect  to  a  World  Coordinate  Frame 


The  matrix  element  w  is  any  non-zero  scale  factor.  In  the  applications  reported 
in  this  thesis  w  will  be  set  equal  to  unity,  producing  the  following  vector: 


(2) 


The  transformation  from  point  A  to  point  B,   illustrated  in  Figure  3.  requires  the 


transformation  matrix  [H]: 


1  00a 
H  =  Trans(x,y,z)  =  g  J  J  J? 


000  1-' 


(3) 


The  translation  from  point  A  to  point  B  is  represented  by  the  vector  v : 

v  =  ai  +  bj  +  ck  (4\ 

The  transformed  vector  is  the  product  of  the  transformation  matrix,  H,  and  the 
original  position  vector,  u. 

v  =  Hlu 


v  = 


100  a 
01  0b 
001  c 
0  0  0  L 


(5) 


The  translation  is  to  be  interpreted  as  the  summation  of  the  vectors  u  and  v. 
This  is  illustrated  in  Figure  3  and  described  in  equation  form  by  equation  (6). 


Figure  3.     Translation  of  a  Point  in  Space  from  Point  A  to  Point  B 


u  +  V  =  Jw  f  +  (w  F  ^w  h  +  -a,:  +  bJ  +  ck 


(6) 


The  second  type  of  transformation  is  the  rotational  transformation.  Given  the 
fixed  coordinate  frame  of  the  previously  discussed  translational  transformation  the 
transformation  corresponding  to  a  rotation,  0,  about  the  x-axis  is: 

10  0  0 

0        cos(6)    -sin(6)        0 
0        sin(0)     cos(6)        0 

(7) 


Rot(x,e>  = 


0 


0 


0 


1 


The  transformation  corresponding  to  a  rotation,  8,  about  the  y-axis  is: 


8 


Rot(y,6)  = 


cos(0) 

0 

sin(8) 

0 

0 

1 

0 

0 

-sin(8) 

0 

cos(0) 

0 

0 

0 

0 

1 

(8) 


The  transformation  corresponding  to  a  rotation,  0,  about  the  z-axis  is: 


Rot(z,0)  = 


cos(0) 

-sin(0) 

0 

0 

sin(0) 

cos(0) 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

(9) 


As  an  illustration  of  a  rotational  transformation  consider  the  following.  Given 
another  point  in  space,  A.  illustrated  in  Figure  4.  if  it  was  desired  to  rotate  the  point  around 
the  z-axis  by  90  degrees  the  Rot(z)  matrix  above  would  be  used.  The  value  for  0,  90 
degrees,  would  be  entered  into  the  matrix,  producing: 


0-1  0  0 

Rot(zf90°)  =  :J8?8. 
L0  00  U 


(10) 


This  matrix  would  multiply  the  vector  representing  the  original  position  of  point 


A: 


u  = 


X 

y 

z 

LlJ 


(11) 


producing  the  new  position  vector,  v,  of  the  point  A. 


v  = 


ro-i  oo~ 

x 

1  000 

y 

0  010 

z 

.0  001. 

LlJ 

(12) 


Figure  4.      Rotation  of  Point  A     90  Degrees  about  the  Z-axis,  Rot(z,90) 


Translational  and  rotational  transformations  are  performed  right  to  left  as  they 
are  read,  for  example: 


Trans(a,b,c)Rot(y,90)Rot(z,90)  = 


10  0a 
0  10b 
0  0  1c 
0   0   0    1 


0   0    10" 
0    10   0 
-10  0   0 
0  0  0    1  J 


0-100 
10  0  0 
0  0  10 
0   0  0    1 


(13) 


The  transformation  illustrated  in  Figure  5  is  a  a  rotation  of  90  degrees  about  the 
z-axis,  a  rotation  of  90  degrees  about  the  y-axis,  a  translation  of  'a'  mm  in  the  x  direction, 
of  'b'  mm  in  the  y  direction,  and  of  'c'  mm  in  the  z  direction. 
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Figure   5.     A  Multiple  Translation  and  Rotation  Transformation  of  Point  A 

It  is  clear  that  an  infinite  number  of  translations  or  rotations  are  permitted.  Each 
transformation  is  simply  represented  by  another  matrix  multiplication  operation. 

2.      Application    of    Basic    Homogeneous    Transformations    to    Simple 
Kinematic  Mechanisms 

Armed  with  the  knowledge  of  homogeneous  translations  and  rotations,  a 
genera,  two  link  kinematic  mechanism  will  be  analyzed.  When  homogeneous 
transformations  are  applied  to  the  linkage  shown  in  Figure  6.  equations  (3)  and  (9)  will  be 
utilized  to  move  coordinate  frame  1,  located  at  point  1,  to  coordinate  frame  2,  located  at 
point  2,  by  specifying  the  multiple  transformation: 

A  =  Trans(dl50,0)  Rot(z,e!)  Trans(d2,0,0)  (!4) 
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The  product  of  the  three  matrix  multiplications,  a  frame  to  frame  homogeneous 
transformation,  is  known  as  an  A  matrix.  Referring  to  Figure  6,  the  coordinate  frame  at 
point  1  has  been  assigned  such  that  the  x-axis  lies  along  the  centerline  of  link  1.  As 
illustrated  in  equation  (14),  the  homogeneous  transformation  moves  the  coordinate  frame 
along  the  x-axis  a  distance  di,  rotates  the  frame  about  the  z-axis  6i  degrees,  and  then 
translates  the  coordinate  frame  a  distance  d2  along  the  newly  rotated  x-axis. 


Figure  6.     A  Two-Link  Kinematic  Mechanism 

For  more  general  link  structures,  which  most  likely  will  be  three  dimensional, 
the  coordinate  frame  transformations  are  more  complex  than  the  example  shown  in  Figure 
6.  Due  to  this  complexity,  a  standardization  of  the  allocation  of  coordinate  frames  and  the 
transformation  process  of  the  coordinate  frames  is  required.  These  standardizations  will  be 
discussed  in  the  next  section. 
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3.      Links,  Joints,  and  Assignment  of  Coordinate  Frames 

Any  manipulator  can  be  described  as  a  series  of  links  connected  together  by 
joints.  A  coordinate  frame  is  placed  on  each  link  in  the  manipulator  and,  as  discussed 
earlier,  homogeneous  transformations  are  used  to  describe  the  relative  position  and 
orientation  difference  between  two  successive  links.  Each  homogeneous  transformation 
matrix  operation  rotates  or  translates  the  coordinate  frame  to  various  positions  in  the 
manipulator. 

The  base  of  a  manipulator  is  defined  to  be  link  0  and  is  not  considered  to  be  one 
of  the  defined  number  of  links  composing  the  manipulator.  In  between  the  base  of  the 
manipulator,  link  0,  and  the  first  joint  of  the  manipulator  is  link  1.  Thus,  link  0  is  attached 
to  link  1  by  joint  1 .  Link  2  is  attached  to  link  1  by  joint  2,  and  so  on. 

A  link  is  characterized  by  two  dimensions:   the  common  normal  distance,  an, 
and  the  angle,  cxn,  between  the  axes  in  a  plane  perpendicular  to  an-   The  common  normal 
distance  is  known  as  the  length  of  the  link  and  the  angle  is  known  as  the  twist  of  the  link. 
These  two  link  dimensions  are  illustrated  in  Figure  7. 
Joint 


Common    nonral   d^^a^ct 


Figure  7.     The  Length,  a,  and  Twist,  a,  of  a  Link 
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To  aid  in  the  visualization  of  these  two  parameters  perform  the  following. 
Visualize  a  small  diameter  bar  of  reasonable  length.  At  each  end  of  the  bar  is  a  joint  which 
pivots.  Now  the  bar  looks  like  a  goal  post  with  short  uprights  and  a  long  crossbar.  Take 
the  uprights  and  bend  them  in.  The  bar  will  appear  as  Figure  8. 


Figure   8.      Illustration  of  Link  Dimensions  a  and  a 

The  next  step  is  to  grasp  the  uprights  and  twist  them  in  opposite  directions.  The 
length  of  the  line  which  connects  the  left  upright  to  the  right  upright,  while  maintaining 
right  angles  with  each,  is  an-  To  visualize  ctn,  draw  a  line  which  is  parallel  to  the  left 
upright  through  the  point  where  the  crossbar  is  attached  to  the  right  upright.  The  angle 
between  the  line  which  has  been  drawn  and  the  right  upright  is  the  angle  of  twist,  an. 

As  stated  earlier,  any  manipulator  is  composed  of  a  series  of  links  which  are 
attached  by  joints.  The  relationship  between  links  is  described  using  the  distance  and  angle 
between  links.  The  distance  between  the  links  is  defined  as  dn  and  the  angle  between  two 
successive  links  is  defined  as  0n.  Refer  to  Figure  9  in  the  following  discussion. 

Two  links  will  be  connected  at  a  joint.  The  joint  will  have  a  joint  axis,  around 
which  the  two  links  will  rotate.  A  normal  from  each  link  intersects  the  joint  axis.  The 
relative  position  of  two  successive  links  is  defined  as  dn,  the  distance  separating  the  points 
of  intersection  on  the  joint  axis  of  the  normals  of  the  two  links  under  consideration.  Define 
the  normal  of  the  link  to  the  left  of  the  joint  as  N\.  Define  the  normal  of  the  link  to  the  right 
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of  the  joint  as  N2-  Extend  Ni  through  the  intersection  of  the  joint  axis.  Draw  a  line 
parallel  and  coplanar  to  NT2  through  the  point  of  intersection  of  Nj  with  the  joint  axis.  The 
angle  between  the  extension  of  Nj  and  the  line  parallel  to  N2  is  defined  as  6n. 


Joint  fi- 


Unk  n  -2 


Figure  9.     Link  Parameters    6,  d,  a,  and  a 

In  order  to  describe  the  relationships  between  links,  a  coordinate  frame  will  be 
assigned  to  each  link,  based  on  the  type  of  joint  to  which  the  link  is  attached.  There  are 
two  types  of  joints,  revolute  and  prismatic. 

Consider  a  revolute  joint,  illustrated  in  Figure  10.  Revolute  joints  are 
characterized  by  6n,  the  joint  variable.  The  origin  of  the  coordinate  frame  of  link  n  is  set  to 
be  at  the  intersection  of  the  common  normal  between  the  axes  of  joints  n  and  n+1,  which 
will  be  the  same  line  as  the  one  measured  for  the  length  of  link  n,  extended  if  necessary, 


15 


and  the  axis  of  joint  n+1.  In  the  case  of  intersecting  joint  axes,  the  origin  is  at  the  point  of 
intersection  of  the  joint  axes.  If  the  axes  are  parallel  the  coordinate  origin  is  not  uniquely 
defined  if  the  above  specifications  are  used,  so  the  origin  is  chosen  to  make  the  joint 
distance  zero  for  the  next  link  whose  coordinate  origin  is  defined.  The  z-axis  for  link  n  will 
be  aligned  with  the  axis  of  joint  n+1.  The  x-axis  will  be  aligned  with  any  common  normal 
which  exists  and  is  directed  along  the  normal  from  joint  n  to  joint  n+1.  In  the  case  of 
intersecting  joints,  the  direction  of  the  x-axis  is  parallel  or  antiparallel  to  the  vector  cross 
product  zn-i  X  zn. 


->->• 


^ZZ7r 


World 


Figure   10.  Revolute  Joint 

Now  consider  prismatic  a  joint,  which  is  illustrated  in  Figure  11.   A  prismatic 
joint  is  characterized  by  the  distance  dn  being  the  joint  variable.  The  direction  of  the  joint 
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axis  is  the  direction  in  which  the  joint  moves.  The  direction  of  the  axis  is  defined  but, 
unlike  a  revolute  joint,  the  position  in  space  is  not  defined.  In  the  case  of  a  prismatic  joint 
the  link  length,  an,  has  no  meaning  and  is  set  to  be  zero.  The  origin  of  the  coordinate 
frame  for  the  prismatic  joint  is  coincident  with  the  next  defined  link  origin.  The  z-axis  of 
the  prismatic  link  is  aligned  with  the  axis  of  joint  n+1.  The  xn-axis  is  parallel  or  antiparallel 
to  the  vector  cross  product  of  the  direction  of  movement  of  the  prismatic  joint  and  zn. 


-*->'. 


World 


Figure   11.   Prismatic  Joint 

4.      The  Denavit-Hartenburg  Transformation 

Now  that  a  coordinate  frame  has  been  assigned  to  each  joint  of  the  manipulator, 
the  next  step  is  to  develop  the  relationship  between  coordinate  frames.  The  relationship 
between  coordinate  frame  n  and  n-1  will  now  be  examined  in  detail.    In  the  calibration 
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processes  reported  in  this  thesis  two  types  of  relationships  will  be  used.  They  are  known 
as  the  modified  Denavit-Hartenburg  (MDH)  transformation,  a  frame  to  axis  transformation, 
and  the  Euler  transformation,  a  frame  to  frame  transformation. 

The  modified  Denavit-Hartenburg  transformation  is  a  derivative  of  the  standard 
Denavit-Hartenburg     transformation.  The     standard      Denavit-Hartenburg     (DH) 

transformation  incorporates  the  translations  and  rotations  introduced  earlier  in  the  thesis, 
and  result  in  an  A  matrix  of  the  following  form: 

An  =  Rot(z,6n)  x  Trans(z,dn)  x  Trans(x,an)  x  Rot(x,an)  /-^n 

This  is  to  be  interpreted  as: 

1 .  A  rotation  of  angle  8n  about  the  z-axis. 

2.  A  translation  of  length  dn  along  the  z-axis. 

3.  A  translation  of  length  an  along  the  newly  rotated  x-axis. 

4.  A  rotation  of  angle  an  about  the  newly  rotated  x-axis. 

The  standard  Denavit-Hartenburg  transformation  is  not  implemented  in  the 
modeling  of  manipulator  arms  for  use  in  the  calibration  procedures  described  in  this  thesis. 
As  described  earlier,  the  location  of  coordinate  frames  are  functions  of  manipulator 
geometry.  Any  variation  in  the  manipulator  geometry  will  cause  a  cascading  change  in  the 
positions  of  the  coordinate  frames.  This  results  in  three  effects  which  are  undesirable  for 
manipulator  calibration: 

1 .  Selection  of  the  base  frame  is  not  arbitrary 

2.  The  zero  position  of  the  manipulator  is  not  arbitrary. 

3.  If  two  joints  having  nominally  parallel  axes  are  found  to  have  non-parallel 
axes,  then  the  transformation  parameters  will  dramatically  change. 
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The  third  result  is  the  most  important.  Many  manipulators  are  designed  to  have 
consecutive  parallel  axes.  In  this  case,  there  is  no  common  normal  between  the  two  axes. 
In  conforming  to  the  rules  introduced  earlier  the  coordinate  frame  is  chosen  such  that  the 
joint  distance  will  be  zero  for  the  next  link  whose  coordinate  link  whose  origin  is  defined. 
In  almost  every  calibration  procedure  a  misalignment  of  the  two  nominally  parallel  joint 
axes  will  be  identified.  A  misaligned  pair  of  joint  axes  produces  a  common  normal 
between  them,  and  the  common  normal  fixes  the  position  of  the  coordinate  frame.  The 
new,  fixed  position  of  the  coordinate  frame  could  be  quite  different  from  the  user  defined 
position  of  the  coordinate  frame  based  on  the  joint  distance  being  zero.  This  could  cause  a 
cascading  change  in  position  of  coordinate  frames  downstream  of  the  coordinate  frame  in 
question.  Such  significant  changes  in  several  positions  of  coordinate  frames  causes 
difficulty  in  the  kinematic  parameter  identification  program. 

The  problem  with  the  parallel  axes  is  eliminated  by  using  the  MDH 
transformation,  which  introduces  a  fifth  matrix  multiplication  in  the  A  matrix,  as  described 
in  the  following  section. 

5.      The  Modified   Denavit-Hartenburg  Transformation 

The  MDH  transformation  is,  as  stated  earlier,  a  frame  to  axis  transformation  and 
is  represented  by  the  following  A  matrix, 

An  =  Rot(z,6n)  x  Trans(z,dn)  x  Trans(x,an)  x  Rot(x,an)  x  Rot(y,pn)  Q6) 

This  is  to  be  interpreted  as: 

1 .  A  rotation  of  angle  0n  about  the  z-axis. 

2.  A  translation  of  length  dn  along  the  z-axis. 

3 .  A  translation  of  length  an  along  the  newly  rotated  x-axis. 

4.  A  rotation  of  angle  an  about  the  newly  rotated  x-axis. 
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An  = 


5 .    A  rotation  of  angle  Pn  about  the  newly  rotated  y-axis. 

In  reduced  matrix  form,  for  a  rotational  joint,  the  operation  is  as  follows: 

cos(0)  -sin(0)cos(a)   sin(6)sin(a)  acos(0) 

sin(0)  cos(0)cos(a)  -cos(0)sin(a)  asin(0) 

0  sin(a)  cos(a)  d 

0  0  0  1 

In  reduced  matrix  form,  for  a  prismatic  joint,  the  operation  is  as  follows: 

cos(0)  -sin(0)cos(a)  sin(0)sin(a)  0 

sin(0)  cos(0)cos(a)  -cos(0)sin(a)  0 

0  sin(a)  cos(a)  d 

0  0  0  1 


(17) 


An  = 


(18) 


6.      The  Euler  Transformation 

The  Euler  transformation  is  a  frame  to  frame  transform  which  requires  six 
parameters: 

An  =  Rot(z,(pn)  x  Rot(y,0n)  x  Rot(x,(pn)  x  Trans(pXD,pyn,pZn)  (19) 

This  will  be  interpreted  as  a  rotation  ^n  about  the  z-axis,  followed  by  a  rotation 
0n  about  the  newly  rotated  y-axis,  followed  by  a  rotation  ^n  about  the  newly  rotated  x- 
axis.  At  the  conclusion  of  these  rotations  the  axes  of  frame  n,  the  coordinate  frame  lo 
which  the  transform  is  occurring,  will  be  perfectly  aligned  with  the  transforming  coordinate 
frame.  The  translations  px,  py,  and  pz  will  move  the  aligned  coordinate  axes  from  the 
origin  of  coordinate  frame  n-1  to  coordinate  frame  n. 
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7.      Application  to  the  PUMA 

It  has  been  established  that  an  A  matrix  is  a  homogeneous  transformation  from 
one  coordinate  frame  to  the  next.  This  A  matrix  can  either  be,  in  applications  described  in 
this  thesis,  a  modified  Denavit-Hartenburg  transformation  matrix  or  an  Euler 
transformation  matrix.  Matrix  A\  will  describe  the  position  and  orientation  of  the  first 
coordinate  frame  with  respect  to  the  world  coordinate  frame.  The  coordinate  frame  attached 
to  link  0  is  translated  and  rotated  such  that  it  is  aligned  in  the  proper  position  and 
orientation,  in  accordance  with  the  rules  described  earlier,  on  link.  1  of  the  manipulator. 
Matrix  A2  will  describe  the  position  and  orientation  of  the  second  coordinate  frame  with 
respect  to  the  first  coordinate  frame.  The  coordinate  frame  attached  to  link  1  is  translated 
and  rotated  such  that  it  is  aligned  in  the  proper  position  and  orientation  on  link  2  of  the 
manipulator.  Thus,  the  position  and  orientation  of  the  second  coordinate  frame  with 
respect  to  the  base  coordinate  frame  is  defined  to  be: 

T2  =  Aj  x  A2  (20) 

Since  the  PUMA  560  manipulator  is  a  six  link  manipulator,  the  T6  matrix, 
which  the  indicate  the  position  and  orientation  of  the  tool  frame  with  respect  to  the  world 
coordinate  frame,  will  be  represented  by: 

T6  =  Ai  x  A2  x  A3  x  A4  x  A5  x  A6  (21) 

Figure  12  illustrates  that  any  coordinate  frame  may  be  reached  from  any  other 
coordinate  frame  by  executing  the  matrix  multiplications  between  the  initial  coordinate 
frame  and  the  final  coordinate  frame,  moving  clockwise  or  counter-clockwise. 
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Figure   12.   Homogeneous  Transformation  Loop 

Moving  clockwise  would  generate  a  forward  kinematic  solution,  equivalent  to 
moving  through  the  manipulator  from,  for  example,  coordinate  frame  3  to  the  tool  frame. 
Moving  counter-clockwise  will  generate  an  inverse  kinematic  solution,  equivalent  to 
moving  backwards  through  the  manipulator  from  coordinate  frame  4  to  coordinate  frame  2. 

With  the  acquired  knowledge  of  links,  joints,  how  to  assign  reference  frames 
and  transform  from  one  reference  frame  to  the  next,  the  PUMA  560's  coordinate  frames 
and  the  nominal  kinematic  parameters  will  be  introduced. 

Figure  13  illustrates  individual  links  and  joints  of  the  PUMA  560. 
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Figure   13.   Links  and  Joints  of  the  PUMA  560  Robot  Manipulator  Arm 

If  all  of  the  rules  governing  the  placement  of  coordinate  frames  are  followed, 
the  position  and  orientation  of  the  coordinate  frame  corresponding  to  each  joint  may  be 
displayed.  Figure  14  illustrates  the  allocation  of  each  coordinate  frame. 
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Figure   14.  PUMA  Frame  Allocation 

The  internal  kinematics  of  the  PUMA  will  remain  constant  regardless  of  the 
world  coordinate  frame  used  or  type  of  measurement  system  employed.  Refer  to  Figure  14 
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to  follow  the  discussion  of  the  transformation  from  the  base  coordinate  frame  of  the 
manipulator  to  frame  5.  Note  that  the  position  of  the  manipulator  depicted  in  Figure  14 
does  not  show  the  manipulator  in  its  zero  position.  The  transforms  which  will  be  described 
in  the  next  paragraph  will  move  from  joint  n-l's  zero  position  to  joint  n's  zero  position. 
All  of  the  transformations  within  the  PUMA  are  MDH  transformations.  In  both  of  the 
calibration  procedures  reported  in  this  thesis  the  transformation  from  the  world  coordinate 
frame  to  the  base  frame  and  the  transformation  from  frame  5  to  frame  6,  the  tool  frame,  is 
an  Euler  transformation.  The  details  of  those  transformations  will  be  discussed  during  the 
simulation  phase  of  each  calibration  procedure. 

1 .  Base  frame,  frame  0,  to  frame  1:  No  rotation  about  Zo,  no  translation  along 

o 

the  Z()-axis,  no  translation  along  the  Xo-axis,  rotate  -90  about  Xq.  This  fixes  coordinate 
frame  1. 

2.  Frame  1  to  frame  2:  No  rotation  about  Z\,  no  translation  along  the  Zi-axis, 
translate  along  the  Xi-axis  431.85  mm,  no  rotation  about  X].   This  fixes  coordinate  frame 

2. 

3.  Frame  2  to  frame  3:    No  rotation  about  Z2,  translate  along  the  Z2-axis 

o 

149.09  mm,  translate  along  the  X2-axis  -20.33  mm,  rotate  90    about  X2.     This  fixes 

coordinate  frame  3. 

4.  Frame  3  to  frame  4:  No  rotation  about  Z3,  translate  along  the  Z3-axis  433.0 

o 

mm,  no  translation  along  the  X3-axis,  rotate  -90   about  X3.  This  fixes  coordinate  frame  4. 

5.  Frame  4  to  frame  5:  No  rotation  about  Z4,  no  translation  along  the  Z4-axis, 

o 

no  translation  along  the  X4-axis,  rotate  90   about  X4.  This  fixes  coordinate  frame  5. 

Since  the  coordinate  frames  have  been  assigned  to  each  joint  and  the 
relationship  between  each  frame  has  been  determined,  a  table  of  nominal  kinematic 
parameters  may  be  constructed.  Note  that  the  transform  from  the  base  frame  to  frame  1  and 
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the  transform  from  frame  5  to  the  tool  frame  are  Euler  transformations.  All  other 
transformations  are  Modified  Denavit-Hartenburg  transformations.  The  bold  entries  are 
defined  to  be  zero. 

TABLE   1.     NOMINAL  KINEMATIC  PARAMETERS  FOR  THE  PUMA  560 


% 

eh 

<Pb 

Pxb 

Pvb 

Pzb 

degrees 

degrees 

degrees 

mm 

mm 

mm 

180.0 

0.0 

90.0 

-394.0 

-383.0 

474.0 

link 

number 

A0, 

di 

ai 

«i 

Pi 

degrees 

mm 

mm 

degrees 

degrees 

1 

2 
3 

4 

5 

0 

0.0 
0.0 
0.0 
0.0 

0 
0 

149.09 

433.00 
0.0 

0.0 

431.85 
-20.33 

0.0 

0.0 

-90.0 

0.0 
90.0 
-90.0 
90.0 

0 

0.0 
0 
0 
0 

<J>6 

66 

<JP6 

Px6 

Py6 

Pz6 

degrees 

degrees 

degrees 

mm 

mm 

mm 

90.0 

0.0 

0.0 

0.0 

0.0 

134.0 

At  this  point  the  concept  of  the  forward  kinematic  solution  and  the  inverse 
kinematic  solution  will  be  introduced.  A  forward  kinematic  solution  is  necessary  to 
determine  the  position  and  orientation  of  the  tool  frame  with  respect  to  the  world  coordinate 
frame  if  all  six  joint  angles  are  known.  Given  six  joint  angles,  the  pose  can  be  determined 
using  a  forward  kinematic  solution.  An  inverse  kinematic  solution  is  necessary  to 
determine  the  six  required  joint  angles  which  would  place  the  the  tool  frame  in  a  specified 
position  and  orientation.  Given  the  tool  pose,  the  six  joint  angles  can  be  determined  using 
an  inverse  kinematic  solution. 
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B.      IDENTIFICATION   METHODOLOGY 
1.      Introduction 

Consider  a  generic  function  in  the  x-y  plane,  illustrated  in  Figure  15. 


Figure   15.   Generic  Function  in  the  x-y  Plane 

If  a  relative  maxima  of  the  function  was  to  be  determined  within  a  range  of  x 
then  the  derivative  of  the  function  could  be  calculated  at  some  x  within  that  range.  If  the 
derivative  was  positive  then  the  maxima  would  be  to  the  right  of  the  x  value  and  if  the 
derivative  was  negative  then  the  maxima  would  be  to  the  left.  The  x  value  would  be 
incremented  and  the  derivative  calculated  until  the  the  derivative  equaled  zero.  The  function 
would  be  optimized  at  this  point  within  the  specified  range  of  x. 

This  concept  can  be  extended  to  surfaces.  The  problem  of  climbing  a  hill  in  the 
most  efficient  manner  is  one  example.  Consider  a  generic  two  variable  function  illustrated 
in  Figure  16. 
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Figure   16.   Example  of  Two-Variable  Function   G(x,y) 


It  is  desired  to  find  maximum  G.  To  determine  the  direction  of  maximum 
increase  of  the  function  G  at  a  specific  point,  calculate  the  gradient  of  the  function  and 
evaluate  the  gradient  at  the  point  in  question: 


dx/xi,y,        ldv'xi,yi 


(22) 


Calculating  the  gradient  provides  the  direction  of  maximum  increase  based  on 
(xl+Ax),  (xl-Ax),  (yl+Ay),  and  (yl-Ay). 

A  question  arises:  How  is  the  gradient  calculated  if  G(x,y)  is  not  specifically 
known?  The  only  way  to  determine  the  gradient  is  through  a  method  of  orderly  estimation 
and  elimination.  For  a  two-dimensional  function  this  provides  a  challenge.   If  the  problem 
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is  extended  to  n-dimensional  space  the  problem  is  extremely  difficult  to  solve.  The 
difficulty  in  n-dimensional  optimization  is  the  development  of  an  algorithm  which  will 
change  the  n  variables  of  the  function  in  an  efficiently  and  orderly  manner  in  order  that  the 
gradient  can  be  determined. 

2.      The  IMSL  routine  ZXSSQ 

The  optimization  procedure  used  in  this  thesis  is  an  IMSL  routine  called 
ZXSSQ.  ZXSSQ  is  a  routine  which  will  vary  the  n  variables  of  a  function  in  order  that  the 
function  will  be  minimized.  As  a  basic  example  of  the  operation  of  ZXSSQ  consider  the 
following  equation: 

y  =  (XixZ)+sin(x2xZ)  (23) 

This  equation  will  be  the  model  of  some  physical  system.   The  parameters  X] 

and  X2  are  to  be  determined  based  on  the  observation  of  y  at  incremental  values  of  Z.  The 

following  observations  are  taken: 

yj  =  1  +  sin  (1)  + (-0.1)=  1.74 
y2  =  2  +  sin(2)  +  (0.1)  =  3.00 
y3  =  3  +  sin(3)  +  (-0.1)=3.04 
y4  =  4  +  sin(4)  +  (0.1)=3.34 
y5  =  5  +  sin(5)  +  (-0.1)=3.94  (24) 

which  is: 

y  =  Xi  x  Z  +  sin(X2  x  Z)  ±  0.1  (25) 


with: 


X1  =  l 

X2  =  1  (26) 


The  model  predicts: 
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yi  =  1  +sin(l)=  1.84 
y2  =  2  +  sin  (2)  =  2.91 
y3  =  3  +  sin(3)=  3.14 
y4  =  4  +  sin  (4)  =  3.24 
y5  =  5  +  sin  (5)  =  4.04 


(27) 


ZXSSQ  will  not  discriminate  the  addition  or  subtraction  of  0.1.  This  is 
equivalent  to  the  presence  of  noise  in  a  measurement  system.  So,  ZXSSQ  will  use  the 
flowchart  in  Figure  17  to  select  the  values  of  X]  and  X2  which  will  best  fit  the  above  data. 


select  new 

Xi  and  X2 


no 


select  initial 
Xi  and  X2 


I 


calculate 

-    '  predicted 


print 
Xi  and  X2 


Figure   17.   Flowchart  of  the  Operation  of  ZXSSQ 
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The  initial  values  of  X\  and  X2  are  defined  by  the  user.  For  each  observation, 
producing  a  yimeasured-  anc*  a  ^predicted  wu*'  be  calculated  based  on  the  current  Xi  and 
X2-  The  difference  between  yipredicted  anc*  yimeasured  w*^  be  calculated  for  each 
observation  and  the  sum  of  these  differences  will  be  calculated.  If  this  sum  is  less  than  the 
user  defined  convergence  criteria  the  current  values  of  X\  and  X2  are  acceptable.  If  not 
ZXSSQ  selects  new  values  for  Xi  and  X2  and  the  process  begins  again.  The  process  of 
the  selection  of  the  values  of  X\  and  X2  performed  by  ZXSSQ  is  a  mathematical 
consideration  which  is  not  important  to  the  results  reported  in  this  thesis,  but  it  must  be 
understood  that  ZXSSQ  makes  the  identification  of  the  kinematic  parameters  of  the  PUMA 
within  a  reasonable  period  period  of  time  possible. 

As  an  illustration  of  the  implementation  of  ZXSSQ  in  this  thesis,  consider  the 
following.  Suppose  a  ball  is  suspended  in  space  in  an  unknown  position  relative  to  a 
defined  set  of  coordinate  axes,  as  illustrated  in  Figure  18. 

(xi.yj.zi) 


(X2,V2,Z2 


,V3,Z3) 


(xcenter»  ^center*  zcenier) 


Figure   18.   Determination  of  the  Center  of  a  Ball  in  Space 
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It  is  possible  to  use  a  touch  probe  to  measure  (x^y^Zi),  the  coordinates  of  an  im 
position  on  the  surface  of  the  ball  relative  to  the  fixed  coordinate  frame.  However,  the  exact 
position  of  the  center  of  the  ball  is  required  in  order  to  assist  in  the  determination  of  the 
pose  of  the  tool.  ZXSSQ  accepts  the  coordinates  of  at  least  three  surface  positions  on  the 
ball  and  calculates  the  coordinates  of  the  center  of  the  ball.  The  procedure  is  as  follows. 

The  end  effector  used  in  the  experimental  phase  of  the  initial  calibration  has  a 
pattern  of  precision  tooling  balls  which  have  a  known  radius,  Rtb.  The  touch  probe  also 
has  a  known  radius,  Rtp.  That  means  that  the  center  of  the  ball  will  be  (Rtb  +  Rtp)  from 
each  measured  position  of  the  touch  probe.  If  only  one  touch  probe  measurement  is  made 
the  actual  position  of  the  center  of  the  ball  could  be  at  any  position  on  a  sphere  of  (Rtb  + 
Rtp)  radius  surrounding  the  single  measurement.  As  more  measurements  are  taken  the 
position  of  the  center  of  the  ball  can  be  determined  more  accurately.  It  has  been  determined 
that  a  minimum  of  three  touch  probe  measurements  are  required  to  accurately  determine  the 
coordinates  of  the  center  of  the  ball.  Each  measurement  produces  an  error  function: 


Fj  =  I  (Rlb  +  Rtp)  -  \  (xc  -xi)2  +  (yc  -ytf  +  (zc  -zrf 


F2  =  |  (Rtb  +  Rlp)  -  %  (xc  -xif  +  (yc  -y2f  +  (^  -zi? 


F3  =  I  (Rtb  +  Rlp)  -  \  (xc  -X3)2  +  (yc  -yif  +  (^  -Z3)2  |  (28) 

ZXSSQ  will  systematically  vary  (xovc<Zc)  until  each  Fj  is  minimized.  If  there 
is  no  noise  in  the  system,  each  Fj  will  be  reduced  to  zero.  If  noise  is  present  the  best  fit 
(Xcyc,Zc)  win  be  calculated.  Once  the  functions  are  minimized,  the  coordinates  of  the 
center  of  the  ball  are  known. 
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3.      Application  to  the  Calibration   Process 

The  kinematic  parameter  identification  of  the  PUMA  560  will  be  performed  as  a 
multi-dimensional  minimization  process  performed  by  ZXSSQ.  The  process  will  proceed 
in  the  following  manner. 

1.  Begin  with  the  nominal  set  of  kinematic  parameters.  This  set  will  most 
likely  be  the  manufacturer's  predicted  parameters  based  on  the  design  and  construction  of  a 
generic  manipulator. 

2.  Select  a  set  of  six  joint  angles,  6}  through  06,  for  the  manipulator. 

3.  Perform  a  forward  kinematic  solution  for  the  PUMA,  using  the  nominal  set 
of  kinematic  parameters.  This  will  calculate  the  predicted  pose  of  the  end  effector  of  the 
manipulator. 

4.  Measure  the  actual  pose  of  the  end  effector  of  the  manipulator.  In  most 
cases  measured  pose  will  be  different  from  the  predicted  pose. 

5.  Modify  the  kinematic  parameters  such  that  the  predicted  pose,  determined 
by  the  forward  kinematic  solution  using  the  modified  kinematic  parameters,  matches  the 
measured  pose  [Ref.  8]. 

This  process  will  be  applied  to  several  sets  of  joint  angles.  The  number  of 
physical  measurements,  meaning  the  number  of  sets  of  joint  angles  required,  must  satisfy: 

Kp  <;  N  x  Df  (29) 

where:        Kp  =  number  of  kinematic  parameters  to  be  identified 

N  =  number  of  measurements  (poses)  taken 
Df  =  number  of  degrees  of  freedom  present  in  each  measurement 
Two  calibration  processes  will  be  reported  in  this  thesis.  Each  of  the  calibration 
procedures  will  have  a  simulation  phase  and  an  experimental  phase.    The  details  of  each 
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kinematic  identification  process  will  be  described  in  each  section  of  the  appropriate 
calibration  procedure.  Refer  to  the  preceeding  paragraph  for  a  general  explanation  of  the 
kinematic  parameter  identification  procedure. 
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III.   FULL  POSE  CALIBRATION 

A.     THEORY 

1.      Introduction 

As  stated  earlier,  the  nominal  coordinate  transformations  from  the  base  frame  to 
frame  5  will  not  change,  regardless  of  the  type  of  calibration  process  used  or  the  type  of 
measurement  system  used  in  the  calibration.  The  transformation  from  the  world  coordinate 
frame  to  the  base  frame  is  an  Euler  transform,  as  well  as  the  transform  from  frame  5  to 
frame  6,  the  tool  frame. 

The  transformation  from  the  world  coordinate  frame  to  frame  1  of  the 
manipulator  needs  to  be  carefully  considered,  since  there  are  potential  parameter 
dependencies  if  certain  transforms  are  chosen.  Consider  Figure  19  which  shows  the  world 
coordinate  frame  xw,yw,zw,  frame  xo,y(XzO>  frame  xb,yb?zb>  an^  frame  xi,yi,zi. 

Frame  xfj.yChzO  is  defined  by  a  DH  transform  from  the  world  frame  to  the  first 
joint  axis  of  the  manipulator,  frame  xb,yb>zb  is  tne  PUMA  manufacturer's  base  frame  and 
frame  xi,y  \,z\  is  the  second  DH  frame  of  the  manipulator.  What  are  the  minimum  number 
of  parameters  that  are  required  to  move  from  the  world  frame  to  frame  xi,yi,z]?  There  are 
two  transformation  paths  which  will  accomplish  this  transform  [Ref.  9]. 

Path  1:  A  DH  transform  from  the  world  coordinate  frame  to  frame  xO.yO^O 
involving  four  parameters  followed  by  another  DH  from  frame  xo,yO>zO  to  frame  xb,yb.zb 
which  will  involve  only  two  parameters  *P  and  d  in  the  transform: 

T£  =  rot(zo,(f>')trans(zo,d')  (30) 
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Figure   19.   Base  Transformations 

Another  DH  transform  from  xb,yt>zb  t0  xl<yi>zl  which  involves  four 
parameters.  Note  that  A0i  and  *  both  rotate  about  zq.  That  means  that  a  rotation 
A6i  about  the  zo-axis  is  indiscernible.  Any  rotation  about  the  zo-axis  can  be  accomplished 
using  an  infinite  number  of  combinations  of  values  of  A61  and  ^ ,  which  means  that 
A61  and  V  can  not  be  identified  independently.  This  leads  to  a  conclusion  that  eight 
independent  kinematic  parameters  are  required  to  move  the  coordinate  frame  from  the  world 
frame  to  the  first  frame  of  the  PUMA  using  this  path. 

Path  2:  A  transform  can  be  defined  directly  from  the  world  coordinate  frame 
xw,yw»Z\V)  to  the  base  frame  xb,yb-zb-  This  is  a  frame  to  frame  transform  so  an  Euler 
transform  is  required: 
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Ab  =  rot(z,(})b)rot(y,eb)rot(x,(pb)trans(pxb,pyb,pzb)  ^\) 

The  DH  transform  from  xb,yb.zb  to  xi,yi,zi  which  will  follow  would 
normally  involve  four  parameters,  but  there  is  another  dependency  involving  A0j  and 
Adi.  A0!  can  be  resolved  into  ^>6b,9b  and  Adj  Can  be  resolved  into  (Pxb'Pyb'Pzb).  This 
reduces  the  number  of  parameters  required  for  the  transform  from  the  base  frame  to  frame  1 
from  four  to  two.  Coupled  with  the  six  parameters  required  to  transform  from  the  world 
frame  to  the  base  frame  it  is  seen  again  that  eight  parameters  are  required  to  transform  from 
the  world  frame  to  frame  1,  but  a  different  set  of  parameters  than  found  in  path  1.  In  this 
simulation  the  second  path  is  chosen. 

The  tool  transform  is  an  Euler  transform,  requiring  the  specification  of  six 
parameters: 

A*  =  rot(z,<f>6)rot(y,66)rot(x,cp6)trans(px6,py6,pz6)  ^2) 

2.      Full     Model     of     the     PUMA,     World      Coordinate      Frame,      and 
Coordinate  Measuring  Machine 

Figure  20  shows  the  full  pose  calibration  apparatus.  The  operation  of  the 
coordinate  measuring  machine  will  be  explained  in  the  experimental  section  of  this  chapter. 
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Figure  20.   Full  Pose  Calibration  Apparatus:      PUMA,  World  Coordinate 
Frame  and  Coordinate  Measuring  Machine 

Table  2  represents  the  kinematic  parameters  of  the  full  pose  model  [Ref.  10].  In 
the  full  pose  model  all  thirty  parameters  not  previously  defined  to  be  zero,  indicated  in 
boldface  type,  are  able  to  be  determined.  Note  the  transform  from  the  world  coordinate 
frame  to  the  base  frame  of  the  PUMA. 
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TABLE  2.     NOMINAL  KINEMATIC  PARAMETERS  FOR  THE  FULL 

POSE  CALIBRATION 


<h 

eh 

<Pb 

Pxb 

Pvb 

Pzb 

degrees 

degrees 

degrees 

mm 

mm 

mm 

180.0 

0.0 

90.0 

-394.0 

-383.0 

474.0 

link 
number 

A8, 

di 

ai 

«i 

Pi 

degrees 

mm 

mm 

degrees 

degrees 

1 

2 

3 
4 

5 

0 

0.0 
0.0 
0.0 
0.0 

0 

0 

149.09 

433.00 
0.0 

0.0 

431.85 
-20.33 

0.0 

0.0 

-90.0 
0.0 
90.0 
-90.0 
90.0 

0 

0.0 
0 
0 
0 

96 

06 

9P6 

Px6 

Pv6 

Pz6 

degrees 

degrees 

degrees 

mm 

mm 

mm 

90.0 

0.0 

0.0 

0.0 

0.0 

134.0 

B.      SIMULATION 

1 .      The  Suite  of  Programs  Used  and  the  Strategy  Involved 

In  performing  a  simulation  study  of  a  proposed  experimental  calibration 
procedure  the  objectives  are  to: 

1.  Confirm  that   the  numerical  algorithm  proposed   for  the  identification 
converges  to  the  correct  values. 

2.  Predict  the  number  of  experimental  poses  required  to  identify  the  kinematic 
parameters  to  a  defined  degree  of  accuracy. 

3.  Estimate  the  resulting  accuracy  of  the  manipulator  if  the  new  kinematic 
model  was  embedded  in  the  control  software  of  the  manipulator  [Ref.  1 1  ]. 
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Several  computer  programs  were  written  to  perform  the  simulation.    These 
programs,  and  their  relationships  to  each  other  are  shown  in  Figure  21 . 


JOINT 


1 


PUMA-VAR.DAT 


] 


PUMA-POS.DAT 


i 


RESULT.DAT 


VERIFY 


CD 

3 

CO 


CO 


POSVER.DAT 


Figure  21.   Flowchart  for  Full  Pose  Simulation 

The  complete  simulation  may  be  regarded  as  a  tool  to  plan  the  experiment  in 
which  the  independent  variables  are  the  number  of  observations  and  the  range  of  joint 
angles  allowed  by  the  common  work  volume  of  the  PUMA  and  the  coordinate  measuring 
machine,  while  the  dependent  variables  are  the  accuracy  of  the  parameter  identification  and 
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the  resulting  manipulator  accuracy  [Ref.  12].    A  detailed  explanation  of  each  program 
follows. 

a.  The  program  JOINT 

Program  JOINT  does  not  require  an  input  data  file. 

Program  JOINT  generates  sets  of  six  joint  angles  which  comprise  a 
simulated  pose  of  the  PUMA.  The  program  can  be  modified  such  that  the  simulated 
working  volume  of  the  PUMA  is  restricted  from  the  maximum  possible  to  either  one-half 
of  one-quarter  of  the  maximum  volume. 

The  joint  angles  are  selected  using  a  random  number  generator.  For 
instance,  if  the  working  volume  of  the  PUMA  has  been  selected  as  the  maximum  possible, 
the  program  takes  the  difference  between  the  extreme  joint  angles  possible,  +  180-(- 
180)=360,  then  multiplies  this  value  by  a  number  between  0  and  1,  which  has  been 
generated  randomly,  thus  generating  one  joint  angle.  Since  six  independent  joint  angles  are 
required,  six  different  random  numbers  and  six  different  joint  angles  are  generated  for  each 
pose,  or  observation. 

The  output  of  program  JOINT  is  a  data  file,  PUMA-VAR.DAT  PUMA- 
VAR.DAT  will  have  n  sets  of  six  joint  angles,  which  comprise  n  simulated  poses  of  the 
PUMA.  In  the  verification  phase  program  JOINT  is  run  again,  with  the  output  data  file 
being  renamed  POSEVER.DAT  for  use  in  program  VERIFY. 

b.  The  program  POSE 

Program  POSE  requires  two  input  data  files,  PUMA-VAR.DAT,  the 
output  data  file  from  program  JOINT,  and  INPUT.DAT,  the  nominal  kinematic  parameters 
of  the  PUMA,  listed  in  Table  1.  POSE  also  inputs  values  "dangle"  and  "dlenth."  The 
value  assigned  to  "dangle"  is  added  to  all  of  the  angular  parameters  except  for  the  ones 
which  are  defined  to  be  zero.   The  value  assigned  to  "dlenth"  is  added  to  all  of  the  length 
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parameters.  Adding  these  values  creates  a  manipulator  which  is  significantly  different  from 
the  manipulator  reflected  in  the  nominal  kinematic  parameter  table,  and  is  equivalent  to 
supplying  the  kinematic  identification  program  with  pseudo-experimental  data.  Creating 
this  different  manipulator  tests  the  integrity  of  the  kinematic  parameter  identification 
program.  Since  the  initial  guess  of  the  parameters  will  be  the  nominal  parameters  the 
identification  program  must  rigorously  solve  for  the  actual  kinematic  parameters,  which 
will  be  the  nominal  parameters  plus  "dangle"  and  "dlenth"  as  appropriate. 

Program  POSE  generates  a  T6  matrix,  representing  the  simulated  position 
and  orientation  of  the  tool  frame  of  the  PUMA,  for  each  set  of  joint  angles  which  were 
generated  by  program  JOINT. 

When  the  joint  angles  are  read  from  PUMA-VAR.DAT  they  are  added  to 
the  parameters  read  from  the  nominal  kinematic  table  which  apply  to  the  individual  joint. 
For  example,  in  POSE  the  variable  TH1  equals  the  sum  of  DTI  and  THETA1.  DTI  is 
input  from  the  nominal  kinematic  table  and  THETA1  has  been  generated  by  JOINT.  The 
nominal  kinematic  parameters  are  added  to  the  simulated  actual  joint  angles  as  any  non-zero 
THETA  value  at  any  joint  represents  a  movement  from  the  zero  position  of  that  joint. 

Once  the  joint  angles  are  calculated  and  the  nominal  kinematic  parameters 
are  adjusted,  a  forward  kinematic  solution  is  calculated.  This  will  produce  one  T6  matrix 

for  each  set  of  joint  angles.  In  order  to  accurately  model  an  actual  measurement  in  the 
laboratory  noise  is  added  to  the  position  vector  of  the  T6  matrix  and  the  six  joint  angles  of 
each  simulated  pose.  The  position  noise  arises  from  the  uncertainty  of  the  precision  with 
which  the  coordinate  measuring  machine  and  associated  software  is  capable  of  identifying 
the  position  of  the  center  of  a  tooling  ball.  The  position  noise  is  generated  by  multiplying  a 
random  number  which  is  generated  in  the  same  manner  as  in  JOINT  by  a  number  read  from 
INPUT.DAT,  "magnx."    The  joint  angle  noise,  called  the  encoder  error,  is  generated  by 
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multiplying  a  random  number  by  another  value  which  is  read  from  INPUT.DAT,  "magnl." 
The  position  noise  is  100  times  the  magnitude  of  the  encoder  noise. 

The  output  of  POSE  will  be  the  six  joint  angles  for  each  simulated  pose 
and  the  corresponding  T^  matrix  for  each  set  of  joint  angles.  This  data  is  stored  in  the 
output  file  PUMA-POS.DAT. 

c.       The  program  ID6 

Program  ID6  requires  two  input  files,  INPUT.DAT  and  PUMA- 
POS.DAT.  The  nominal  kinematic  parameters  are  read  from  INPUT.DAT  in  order  to 
provide  the  kinematic  parameter  identification  with  a  beginning  point.  The  T6  matrices 
contained  in  PUMA-POS.DAT  which  were  generated  by  program  POSE  contain  the 
simulated  poses  with  which  the  actual  kinematic  parameters  will  be  identified  in  this 
program. 

ZXSSQ  will  perform  the  kinematic  parameter  identification.  The  function 
to  be  minimized  by  ZXSSQ  is  actually  an  array  of  six  functions.  Each  pose  will  have  six 
functions  to  be  minimized,  so  if  there  are  ten  poses,  or  observations,  ZXSSQ  will  have 
sixty  functions  to  minimize  using  thirty  variables. 

A  subroutine  within  ZXSSQ,  PUMA_ARM,  calculates  the  T6  matrix 
corresponding  to  the  current  kinematic  parameters  for  each  set  of  six  joint  angles  input  from 
PUMA-POS.DAT.  The  simulated  measured  T6  matrix  read  from  PUMA-POS.DAT  for 
each  set  of  joint  angles  is  known.  Since  the  original  nominal  kinematic  parameters  were 
altered  before  the  simulated  measured  T6  matrix  was  calculated,  the  simulated  measured  T6 
matrix  will  be  different  from  the  T6  matrix  calculated  using  the  generated  joint  angles  and 
the  nominal  kinematic  parameters.  Also,  the  added  measurement  noise  is  present, 
providing  more  of  a  difference  in  the  two  matrices.  For  each  observation  the  array  of  six  F 
functions  is  calculated.  After  performing  the  matrix  subtraction: 
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AT  =  T 


measured. 


L  ^calcu 


aled. 


(33) 


the  six  F  functions  are  determined  for  each  observation: 

Fj  =  AT(1,4) 
F2  =  AT(2,4) 
F3  =  AT(3,4) 
(AT(3,2)  -  AT(2,3)) 


F4  = 


x  scaling  factor 


F5  =  - —  '       x  scaling  factor 

„      (AT(2,1)  -  AT(1,2))  ..      f    t 

¥(,  =  - — - — '— — —  x  scaling  factor 


(34) 


The  scaling  factor  which  multiplies  the  functions  in  equation  (34)  is  used 
to  remove  the  emphasis  from  the  position  error.  The  position  error  in  a  pose  will  be  much 
larger  than  the  orientation  error  between  two  T6  matrices.  For  this  reason  the  emphasis  in 
the  optimization  program  will  be  on  eliminating  the  position  error,  while  largely  ignoring 
the  orientation  error.  This  is  unacceptable,  since  the  orientation  error  is  equally  important 
as  the  position  error.  The  scaling  factor  places  the  orientation  error  at  the  same  level  of 
magnitude  as  the  position  error. 

The  F  functions  in  equation  (34)  represent  the  error  matrix  [Ref.  13]: 


A  = 


0 

-6Z 

6y 

dx 

&z 

0 

-Ox 

dy 

-6y 

Ox 

0 

dz 

0 

0 

0 

0    J 

(35) 


It  must  be  noted  that  equation  (35)  can  only  be  used  when  the  two 
matrices  being  subtracted  are  almost  equal.  In  an  actual  experimental  process  the  measured 
poses  usually  significantly  differ  from  the  predicted  pose.   This  difference  causes  the    A 
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matrix,  equation  35,  to  have  relatively  large  elements.  ZXSSQ  is  often  unable  to  converge 
under  these  conditions.  The  different  version  of  ID6  will  be  introduced  in  the  Experimental 
section  of  this  chapter. 

After  all  the  observations  have  been  calculated  for  each  pose,  ZXSSQ 
compares  each  F  value  with  a  user  defined  convergence  criteria.  Convergence  is  defined 
when  the  kinematic  parameters  selected  from  one  iteration  to  the  next  agree  to  four 
significant  digits.  If  every  F  for  each  pose  is  less  than  this  convergence  criteria,  then  the 
current  kinematic  parameters  are  saved  as  the  correct  parameters.  If  not,  ZXSSQ  changes 
the  thirty  kinematic  parameters  and  performs  the  process  again  until  the  convergence  criteria 
is  satisfied. 

The  output  of  ID6  is  RESULT.DAT.  The  identified  kinematic  parameters 
will  be  stored  in  the  same  format  as  INPUT.DAT.  In  the  simulation  the  identified 
kinematic  parameters  will  be  the  parameters  of  INPUT.DAT  plus  dangle  for  orientation 
parameters  and  dlenth  for  length  parameters  plus  some  small  error  value  created  by  the 
addition  of  the  simulated  measurement  noise. 
d.      The  program   VERIFY 

Program  VERIFY  requires  three  input  programs.  The  first, 
POSEVER.DAT,  is  the  output  from  program  JOINT,  and  includes  sets  of  six  joint  angles. 
The  other  two  are  kinematic  parameters,  INPUT.DAT  and  RESULT.DAT.  INPUT.DAT, 
as  explained  earlier,  contains  the  nominal  kinematic  parameters,  and  RESULT.DAT 
contains  the  kinematic  parameters  identified  in  program  ID6. 

VERIFY  measures  the  position  and  orientation  difference  between  poses 
calculated  using  the  nominal  kinematic  parameters  and  the  nominal  kinematic  parameters. 
For  each  set  of  joint  angles,  VERIFY  calculates  a  forward  kinematic  solution  using  both  the 
nominal   and   identified  kinematic  parameters  and  takes  the  difference   between   the 
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corresponding  T^  matrices.  The  position  error  and  orientation  error  for  all  of  the  sets  of 
joint  angles  are  added  and  these  values  are  divided  by  the  total  number  of  sets  of  joint 
angles.  This  will  produce  the  total  position  error  and  total  orientation  error  for  a  given 
number  of  observations. 

C.     EXPERIMENTATION 

1.      The  Tooling  Ball  End  Effector 

The  tooling  ball  end  effector  is  illustrated  in  Figure  22.  Its'  purpose  is  to 
provide  a  means  for  determining  the  position  and  orientation  of  frame  6  of  the  PUMA.  The 
five  balls  attached  to  the  effector  are  precision  tooled  to  a  radius  of  6.35  mm.  This  known 
radius,  along  with  the  known  radius  of  the  touch  probe  of  the  coordinate  measuring 
machine  provides  the  essential  information  required  for  identification  of  the  center  of  each 
tooling  ball  by  the  routine  BALL  described  earlier  in  the  thesis.  Also,  each  of  the  four 
tooling  balls  contained  in  the  plane  normal  to  the  axis  of  the  effector  are  nominally  90 
degrees  apart,  creating  orthogonality  between  any  two  in  series  around  the  circumference  of 
the  tool.  The  fifth  tooling  ball's  center  lies  on  an  axis  which  passes  the  two  lines  which 
attach  balls  on  opposite  sides  of  the  tool,  creating  another  orthogonality  between  the  fifth 
ball  and  each  of  the  other  four.  The  orientation  of  frame  six  is  fixed  on  the  tool  and  is 
required  information  for  the  determination  of  the  position  and  orientation  of  the  tool  frame 
by  program  CMMPOSE. 
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Figure  22.  Tooling  Ball   End   Effector 

2.      The  Coordinate  Measuring  Machine  and  World  Coordinate  Frame 
a.      Construction 

The  coordinate  measuring  machine  (CMM)  used  for  data  acquisition  in 
this  thesis  is  illustrated  in  Figure  23. 
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Y-axis  receptacle 


Z-axis  receptacle 


X-axis  receptacle 


Figure  23.  Coordinate  Measuring  Machine 

The  x-axis  lies  on  the  base  of  the  CMM.  Rotating  the  x  wheel  will  move 
the  x-axis  receptacle  to  the  left  or  right,  changing  the  x-value  displayed.  The  y-axis  is 
contained  within  a  post  which  is  attached  to  the  slide  that  changes  the  x  position.  Rotating 
the  y  wheel  moves  the  y-axis  receptacle  up  or  down,  changing  the  y-value  displayed.  The 
z-axis  is  contained  is  the  y-axis  receptacle  assembly.  Rotating  the  z  wheel  moves  the  z 
receptacle  in  or  out,  changing  the  z  value  displayed.  Consider  the  point  (1,1,1)  relative  to  a 
known  world  reference  frame.  Rotating  the  x  wheel  will  produce  a  change  in  position 
(Ax,l,l).  Rotating  the  y  wheel  will  produce  a  change  in  position  (l,Ay,l).    Rotating  the  z 
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wheel  will  produce  a  change  in  position  (l,l,Az).   It  is  clear  that  any  point  in  the  working 
volume  of  the  CMM  may  be  reached  by  the  touch  probe. 

A  significant  advantage  of  this  CMM  is  its'  ability  to  zero  at  any  position. 
Each  axis  may  be  zeroed  independently  or  concurrently.  This  is  important  in  identifying 
the  location  of  the  world  coordinate  frame.  A  diagram  of  the  world  coordinate  frame  used 
in  the  full  pose  calibration  is  shown  in  Figure  24. 


position  of 
world  coordinate  frame 


aspect  of  viewer 
Figure  24.  The  Full  Pose  World  Coordinate  Frame  Cube 

The  cube  itself  does  not  actually  contain  the  world  coordinate  frame. 
Using  the  three  faces  of  the  cube  which  form  the  corner  of  the  bottom  of  the  cube  closest  to 
and  most  left  of  the  operator,  also  identified  in  Figure  24,  allow  for  a  procedure  which 
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identifies  point  (0,0,0)  of  the  world  coordinate  frame.  Once  this  point  is  identified,  the 
axes  of  the  world  coordinate  frame  match  the  axes  of  the  CMM.  The  procedure  for 
identifying  the  world  coordinate  frame  is  as  follows.  Note  that  the  order  of  the 
identification  process  of  the  world  coordinate  frame  is  arbitrary. 

1 .  The  touch  probe  is  positioned  in  area  near  the  bottom,  left,  forward 
face  of  the  cube. 

2.  Activate  the  'x-zero'  mode  of  the  CMM.  Move,  using  all  three  axes 
if  necessary,  the  touch  probe  such  that  its'  position  is  as  close  to  the  forward  corner  of  the 
bottom  of  the  left  face  as  possible.  Slowly  move  the  probe  to  the  right,  using  the  x-axis 
wheel  only,  until  contact  is  made  with  the  forward  face  of  the  cube.  When  contact  is  made 
the  display  unit  (DU)  will  beep  and  the  x  position  will  indicate  zero. 

3.  Activate  the  'y-zero'  mode  of  the  CMM.  Move,  using  all  three  axes 
if  necessary,  the  touch  probe  such  that  its'  position  is  as  close  to  the  forward,  left  corner  of 
the  bottom  face  of  the  cube  as  possible.  Slowly  move  the  probe  upwards,  using  the  y-axis 
wheel  only,  until  contact  is  made  with  the  bottom  of  the  cube.  When  contact  is  made  the 
DU  will  beep  and  the  y  position  will  indicate  zero. 

4.  Activate  the  'z-zero'  mode  of  the  CMM.  Move,  using  all  three  axes 
if  necessary,  the  touch  probe  such  that  its'  position  is  as  close  to  the  bottom,  left  corner  of 
the  forward  face  of  the  cube  as  possible.  Slowly  move  the  probe  towards  the  cube,  using 
the  z-axis  wheel  only,  until  contact  is  made  with  the  forward  face  of  the  cube.  When 
contact  is  made  the  DU  will  beep  and  the  y  position  will  indicate  zero. 

Once  this  procedure  is  completed  a  point  in  space  located  a  distance  equal 
to  the  diameter  of  the  touch  probe  along  each  axis,  vjxdia(^|ydia1J.|Zdia,J^  wjh  be  identified  as 
the  location  of  the  world  coordinate  frame.  This  process  is  easily  repeatable  and  has  a  high 
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degree  of  accuracy,  so  for  different  sets  of  experimental  measurements  the  position  of  the 
world  coordinate  frame  should  not  vary  to  a  significant  degree. 

b.      Data  Acquisition   Using  the  Coordinate  Measuring  Machine 

Data  acquisition  using  the  coordinate  measuring  machine  consists  of  a 
repetitive  measurement  of  the  position  of  three  different  tooling  balls  on  the  end  effector 
illustrated  in  Figure  22.  The  PUMA  is  operated  in  joint  mode,  which  allows  each  of  the  six 
joints  to  be  rotated  independently.  The  end  effector,  which  is  attached  to  the  tooling  ball 
assembly,  is  moved  to  a  variety  of  positions.  When  the  operator  is  satisfied  with  the 
position  of  the  tool,  the  measurement  process  begins. 

Execution  of  the  pose  measurement  program,  CMMPOSE,  begins  by 
inquiring  which  tooling  ball  on  the  tool  is  being  measured.  This  information  is  required  in 
order  to  determine  the  orientation  of  frame  6.  The  touch  probe  of  the  CMM  is  moved 
slowly  towards  the  selected  tooling  ball.  As  the  probe  touches  the  surface  of  the  ball  the 
display  unit  of  the  CMM  beeps.  The  position  displayed  by  the  unit  is  the  position  of  the 
probe  at  the  first  instant  that  the  probe  touches  the  surface  of  the  ball,  and  is  displayed  in 
millimeters  to  two  significant  digits.  If  the  probe  slightly  slides  after  initial  contact  the 
displayed  position  will  not  change,  remaining  until  one  of  the  three  axes  of  the  CMM  is 
moved.  CMMPOSE  asks  for  three  touch  probe  measurement  sets  per  ball  and  determines 
the  center  of  each  tooling  ball  using  the  procedure  described  in  section  I1.B.2.  Once  the 
center  of  each  ball  is  calculated  a  residual  error  reflecting  the  uncertainty  of  the  accuracy  of 
the  predicted  position  of  the  center  of  the  ball  is  displayed.  An  option  of  storing  or  purging 
the  data  is  given  at  this  point.  Based  on  the  judgement  of  the  operator,  the  data  is  sent  for 
further  manipulation  by  CMMPOSE,  or  the  data  is  purged  and  another  set  of  ball 
measurements  is  taken.  An  acceptable  residual  error  has  a  value  near  0.000001  mm.  Once 
the  center  of  three  of  any  of  the  five  tooling  balls,  Pj,  P2,  and  P3,  is  determined,  the 
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program  synthesizes  the  position  of  the  center  of  a  fourth  ball,  P4,  by  calculating  the  vector 
cross  product  [Ref.  14]: 

P4  =  (P3  -  Pi)  x  (P2  -  Pi)  (36) 

The  relationship  between  the  coordinates  of  the  center  of  each  ball 
expressed  in  terms  of  the  tool  frame  and  the  world  coordinate  frame  is 

[Pf]  =  [A][Pi]  (37) 

where  Pj  is  the  4x1  column  vector  of  the  coordinates  of  the  itn  ball 
expressed  with  respect  to  the  world  frame,  Pj  is  the  4x1  column  vector  of  the  coordinates 
of  the  im  ball  with  respect  to  the  tool  frame,  and  A  is  the  4x4  homogeneous  transformation 
matrix  from  the  world  frame  to  the  tool  frame. 

If  Pj  is  known  by  precalibrating  the  tool,  and  Pj  is  measured  then  A  may 
be  calculated  and  used  as  the  measured  pose  in  the  calibration  process.  The  difficulty  arises 
in  inverting  equation  (37)  to  obtain  A.  The  following  information  is  known: 

[p;]=[a][pi] 

K  =  [A]  [P2J 
[pJJ  =  [A]  fP3] 
[P;j  =  [A]  [P4]  (38) 


Algebraic  manipulation  produces: 


Pi-    P2-    P3-    P4    =  iA  [Pi]    P2.    P3,  LP4. 


(39) 


reducing: 


M  =  [A][P] 


(40) 


52 


Since  P  ,  A,  and  P  are  all  4x4  matrices,  equation  (40)  can  be  inverted, 
producing  matrix  A,  the  pose  of  the  tool: 

[A]-[F'][r»]  (41) 

Two  operators  can  expect  to  measure  one  pose  in  ten  minutes.  One 
operator  will  be  utilized  to  move  the  PUMA  and  operate  the  CMM  and  the  other  operator 
will  input  data  as  instructed  by  program  CMMPOSE. 

c.      Identification  of  Actual  Kinematic  Parameters 

A  modified  version  of  program  ID6  is  used  for  the  identification  of  the 
actual  kinematic  parameters  of  the  PUMA.  The  modification  is  in  the  functions  which  are 
minimized  by  the  IMSL  routine  ZXSSQ.  As  mentioned  earlier,  the  delta  matrix  of  equation 
(35)  is  suitable  for  use  only  when  the  predicted  pose  and  the  measured  pose  are  quite 
close.  Experimentation  will  usually  produce  matrices  which  are  dissimilar.  For  this 
reason,  each  element  of  the  matrix  resulting  from  the  difference  between  the  predicted  and 
measured  pose  is  minimized: 

¥i      F4      F7     Fio  "         an     a12     a13     a14  an     a12     a13     a14 


F2      F5      Fg     Fn 

F3      F6      F9     F12 

0        0        0        1 


^21   322   223   324 
331   332   333   a34 


321   322   323   324 
331   332   a33   a34 


0  0  0  1      jpredicted     L      0  0  0  1      jmeasured 

(42) 


This  produces  twelve  functions  which  are  to  be  minimized.  This  method 
provides  a  more  rigorous  method  for  the  identification  of  the  kinematic  parameters.  Table  3 
shows  the  kinematic  parameters  identified  using  the  full  pose  calibration  procedure  [Ref. 
15]. 
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TABLE  3.     FULL  POSE  IDENTIFIED  PARAMETERS 


Parameter 

Nominal  Value 

Identified  Value 

<Pb 

180.0 

179.9579 

eh 

0.0 

1.5120 

<pb 

90.0 

89.0219 

Pxb 

-394.0 

-393.9838 

Pvb 

-383.0 

-405.0608 

Pzb 

474.0 

466.8381 

ai 

0.0 

-0.04923 

ai 

-90.0 

-89.9977 

66^ 

0.0 

-0.4888 

a-> 

431.9 

432.1216 

o> 

0.0 

-0.0303 

P: 

0.0 

-0.01515 

69, 

0.0 

-1.2069 

d, 

149.1 

149.1455 

ai 

-20.3 

-19.2270 

a^ 

90.0 

90.0512 

664 

0.0 

-0.9144 

d4 

433.0 

432.8899 

a4 

0.0 

0.0040 

a4 

-90.0 

-89.9909 

60s 

0.0 

2.2364 

ds 

0.0 

-0.6629 

as 

0.0 

-0.0258 

as 

90.0 

89.9345 

<t>6 

90.0 

91.2400 

6* 

0.0 

-0.0979 

qp6 

0.0 

-0.0575 

Px6 

0.0 

0.1863 

Py6 

0.0 

-0.2329 

Pz6 

134.0 

133.1557 
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IV.    PARTIAL  POSE  CALIBRATION 

A.     THEORY 

1 .      Introduction 

As  slated  earlier,  the  nominal  coordinate  transformations  from  the  base  frame  of 
the  manipulator  to  frame  5  will  not  change.  These  transformations  are  inherent  to  the 
manipulator  and  are  independent  of  the  pose  measurement  scheme  being  employed.  The 
transformations  from  the  world  coordinate  frame  to  the  base  frame  and  from  frame  5  to 
frame  6  are  the  same  transformations  as  employed  in  the  full  pose  calibration. 

The  world  coordinate  frame  is  not  in  the  same  position,  and  is  not  defined  in  the 
same  manner,  as  it  was  in  the  full  pose  calibration.  The  partial  pose  calibration 
measurement  system  uses  a  linear  slide  to  which  the  tool  frame  of  the  manipulator  is 
attached.  The  only  variable  which  is  measured  is  the  position  of  the  slide  relative  to  a  zero 
position  defined  at  the  beginning  of  each  pose  measurement  session.  The  world  frame  is 
defined  as  the  T6  matrix  of  the  tool  when  the  slide's  position  on  the  slide  equals  zero.  The 
slide  acts  as  a  prismatic  joint  and  hence  is  defined  only  in  direction.  The  location  of  the 
axis  is  undefined.  The  three  translational  components  of  the  transform  from  frame  five  to 
frame  six,  M,  are  arbitrary  and  may  be  set  equal  to  zero.  This  defines  that  the  origins  or 
frame  five  and  six  are  coincident,  as  shown  in  Figure  25.  This  will  reduce  the  full  thirty 
parameter  model  to  twenty-seven  parameters. 

The  world  coordinate  frame's  orientation  is  not  known,  either.  The  direction  of 
one  axis  is  able  to  be  defined.  In  this  experiment  the  axis  of  the  linear  slide  has  been 
defined  as  the  x  axis.  The  y  and  z  axes  of  the  world  coordinate  frame  are  orthogonal  to  the 
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x-axis,  but  their  orientation  relative  to  the  x-axis  is  undefinable,  arbitrary,  and  therefore  set 
equal  to  zero.  This  further  reduces  the  model  to  twenty-six  parameters. 


Axis  o-f  L'fteor  Slide 


Figure  25.   Linear  Slide  Transformations 

The  partial  pose  calibration  model  dictates  that  the  tool  frame  pose  is  invariant. 
Each  pose  will  have  the  following  orientation: 


T*  = 


1    0   0  X 

0   0-10 

0    10   0 

.000    1  J 


(43) 


This  T6  matrix  defines  the  x-axis  of  the  tool  frame  to  be  in  the  same  direction  as 
the  x-axis  of  the  world  coordinate  frame,  the  y-axis  of  the  tool  frame  to  be  in  the  same 
direction  as  the  z-axis  of  the  world  coordinate  frame,  and  the  z-axis  of  the  tool  frame  to  be 
in  the  same  direction  as  the  negative  y-axis  of  the  world  coordinate  frame.     Also,  the 


56 


position  of  the  origin  of  frame  six  is  at  position  (X,0,0),  where  X  is  the  position  of  the 
linear  slide.  Note  that  it  is  still  not  possible  to  uniquely  define  the  orientation  of  the  world 
coordinate  frame  with  respect  to  any  frame  on  the  PUMA.  It  is  only  known  that  the 
orientation  of  the  world  coordinate  frame  is  consistently  defined  with  respect  to  the 
orientation  of  the  tool  frame. 

2.      Full  Model  of  the  Puma  and  the  Linear  Slide 

Figure  26  shows  the  partial  pose  calibration  apparatus.  The  operation  of  the 
coordinate  measuring  machine  as  a  linear  slide  will  be  explained  in  the  experimental  section 
of  this  chapter. 


Figure  26.  Partial  Pose  Calibration  Apparatus:     PUMA  and  Linear  Slide 
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Table  4  displays  the  kinematic  parameters  of  the  partial  pose  model.  In  the 
partial  pose  model  twenty-six  parameters  are  able  to  be  determined.  The  parameters  in 
boldface  type  are  unable  to  be  experimentally  determined  consequently  their  value  is 
defined  as  equaling  zero,  or,  in  the  case  of  the  four  parameters  in  the  sixth  transformation, 
the  parameters  are  not  independent,  as  explained  in  the  previous  section.  Note  the 
transform  from  the  world  coordinate  frame  to  the  base  frame  of  the  PUMA  has  changed 
from  the  full  pose  calibration.  The  world  coordinate  frame  is  not  at  the  corner  of  the  cube, 
as  it  was  in  the  full  pose  calibration.  The  nominal  kinematic  parameters  within  the  PUMA 
do  not  change,  regardless  of  the  type  model  used  in  the  calibration  process. 

TABLE  4.     NOMINAL  KINEMATIC  PARAMETERS  FOR  THE  PARTIAL 

POSE  CALIBRATION 


<f>b 

eh 

<Pb 

Pxb 

Pyb 

Pzb 

degrees 

degrees 

degrees 

mm 

mm 

mm 

151.0 

-20.0 

90.0 

-203.0 

-254.0 

457.2 

link 

number 

A0, 

di 

ai 

aj 

Pi 

degrees 

mm 

mm 

degrees 

degrees 

1 
2 
3 
4 

5 

0 

0.0 
0.0 
0.0 
0.0 

0 
0 

149.09 

433.00 

0.0 

0.0 
431.85 
-20.33 

0.0 

0.0 

-90.0 
0.0 
90.0 
-90.0 
90.0 

0 

0.0 
0 
0 
0 

<fc> 

06 

<P6 

Px6 

Pv6 

Pz6 

degrees 

degrees 

degrees 

mm 

mm 

mm 

90.0 

0.0 

0.0 

0.0 

0.0 

0.0 
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B.      SIMULATION 

1.      The  Suite  of  Programs  Used  and  the  Strategy  Involved 

As  stated  earlier,  the  simulation  study  is  performed  to: 

1.  Confirm  that  the  numerical  algorithm  proposed  for  the  identification 
converges  to  the  correct  values. 

2.  Predict  the  number  of  experimental  poses  required  to  identify  the  kinematic 
parameters  to  a  defined  degree  of  accuracy. 

3.  Estimate  the  resulting  accuracy  of  the  manipulator  if  the  new  kinematic 
model  was  embedded  in  the  control  software  of  the  manipulator. 

In  the  partial  pose  calibration,  the  forementioned  "correct"  values  are  the 
kinematic  parameters  identified  using  the  full  pose  method.  The  computer  programs 
written,  and  their  relationships,  are  illustrated  in  Figure  27. 


PUMA-POS.DAT 


ID6_linsc 


RESULT.DAT 


Figure  27.  Flowchart  for  Partial  Pose  Simulation 
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The  complete  simulation  may  be  regarded  as  a  tool  to  plan  the  experiment  in 
which  the  independent  variables  are  the  number  of  observations  and  the  range  of  joint 
angles  allowed  by  the  common  work  volume  of  the  PUMA  and  the  linear  slide,  while  the 
dependent  variables  are  the  accuracy  of  the  parameter  identification  and  the  resulting 
manipulator  accuracy.  A  detailed  explanation  of  each  program  follows. 

a.       The  program  LI  NSC 

Program  LINSC  requires  the  input  file  INPUT.DAT,  the  nominal 
kinematic  data  for  the  PUMA,  which  are  listed  in  Table  1.  LINSC  also  inputs  values 
"dangle"  and  "dlenth,"  as  did  program  POSE  in  the  simulation  phase  of  the  full  pose 
calibration.  It  will  be  instructive  to  reexamine  the  function  of  these  two  parameters.  The 
value  assigned  to  "dangle"  is  added  to  all  of  the  angular  parameters  except  for  the  ones 
which  are  defined  to  be  zero.  The  value  assigned  to  "dlenth"  is  added  to  all  of  the  length 
parameters.  Adding  these  values  creates  a  manipulator  which  is  significantly  different  from 
the  manipulator  reflected  in  the  nominal  kinematic  parameter  table,  and  is  equivalent  to 
supplying  the  kinematic  identification  program  with  pseudo-experimental  data.  Creating 
this  different  manipulator  tests  the  integrity  of  the  kinematic  parameter  identification 
program.  Since  the  initial  guess  of  the  parameters  will  be  the  nominal  parameters  the 
identification  program  must  rigorously  solve  for  the  actual  kinematic  parameters,  which 
will  be  the  nominal  parameters  plus  "dangle"  and  "dlenth"  as  appropriate. 

Program  LENSC  generates  six  joint  angles  which  will  produce  a  T6 

matrix  with  an  orientation  that  is  predefined  and  fixed  and  a  position  on  the  slide  which  has 
been  randomly  generated.  This  is  done  by  performing  an  inverse  kinematic  solution,  since 
the  T6  matrix  is  known.  The  set  of  joint  angles,  when  used  with  the  nominal  kinematic 
parameters  of  the  PUMA,  which  will  produce  the  required  T6  matrix  is  the  required  data. 
The  EMSL  routine  ZXSSQ  is  again  used  to  make  this  determination. 
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ZXSSQ  uses  an  initial  guess  of  all  six  joint  angles  equaling  zero.  A 
forward  kinematic  solution  is  calculated  based  on  these  joint  angles  and  the  nominal 
kinematic  parameters  plus  dangle  and  dlenth.  The  difference  between  each  element  of  the 
calculated  T6  matrix  and  the  required  T6  matrix  is  calculated.  These  elements  represent  the 
functions  to  be  minimized: 


Fi  F4  F7  F10 

F2  F5  Fg  Fn 

F3  F6  F9  F12 

0  0  0  1 


an  a12  a13  a14 

a2i  a22  a23  a24 

331  332  333  334 

0  0  0  1 


predicted 


3ll  312  a13  a14 

32i  322  a23  a24 

331  332  a33  a34 

0  0        0  1 


j  measured 

(44) 


ZXSSQ  modifies  the  joint  angle  set  until  a  convergence  criteria  is 
satisfied.  At  convergence  the  required  T6  matrix  and  the  six  joint  angles  are  returned  to  the 
main  program. 

It  has  been  stated  that  in  order  to  accurately  model  an  actual  measurement 
in  the  laboratory  noise  must  be  added  to  the  simulated  T6  matrix  and  the  six  joint  angles  of 
each  pose.  The  position  noise  and  encoder  noise  is  generated  in  the  same  manner  as  in 
program  POSE  in  the  full  pose  simulation.  The  position  noise  in  the  partial  pose  simulation 
arises  from  the  uncertainty  in  the  measurement  of  the  position  of  the  slide.  The  orientation 
error  arises  from  inconsistencies  in  the  bar  supporting  the  slide.  The  bar  almost  certainly 
will  have  elements  of  twist  and  bend  in  it.  These  elements  will  cause  differences  in 
orientation  as  the  slide  moves  along  its'  length. 

After  the  noise  is  added  to  the  T6  matrix  and  the  joint  angles,  the  data  is 
stored  in  the  output  file  PUMA-POS.DAT 

b.      The  program  ID6J.INSC 

Program  ID6_linsc  requires  two  input  files,  INPUT.DAT  and  PUMA- 
POS.DAT.    The  nominal  kinematic  parameters  are  read  from  INPUT.DAT  in  order  to 
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provide  the  kinematic  parameter  identification  with  a  beginning  point.  The  T/6  matrices  and 
associated  joint  angles  contained  in  PUMA-POS.DAT  which  were  generated  by  program 
LINSC  contain  the  simulated  poses  with  which  the  actual  kinematic  parameters  will  be 
identified  in  this  program. 

ZXSSQ  will  perform  the  kinematic  parameter  identification.  Each  element 
of  the  matrix  resulting  from  the  difference  between  the  predicted  and  measured  pose  is 
minimized,  as  was  done  during  the  actual  kinematic  parameter  identification  of  the  full  pose 
calibration  method.  See  equation  (43)  for  the  equation  of  the  functions  to  be  minimized. 

This  produces  twelve  functions  which  are  to  be  minimized.  Subroutine 
PUMA_ARM  calculates  the  T6  matrix  corresponding  to  the  current  kinematic  parameters 
for  each  set  of  six  joint  angles  input  from  PUMA-POS.DAT.  The  simulated  measured  T/6 
matrix  read  from  PUMA-POS.DAT  for  each  set  of  joint  angles  is  known.  Since  the 
original  nominal  kinematic  parameters  were  altered  before  the  simulated  measured  T6 
matrix  was  calculated,  the  simulated  measured  T6  matrix  will  be  different  from  the  T6 
matrix  calculated  using  the  generated  joint  angles  and  the  nominal  kinematic  parameters. 
Also,  the  added  measurement  noise  is  present,  providing  more  of  a  difference  in  the  two 
matrices. 

After  all  the  observations  have  been  calculated  for  each  pose,  ZXSSQ 
compares  each  F  value  with  a  user  defined  convergence  criteria.  Convergence  is  defined 
when  the  kinematic  parameters  selected  from  one  iteration  to  the  next  agree  to  four 
significant  digits.  If  every  F  for  each  pose  is  less  than  this  convergence  criteria,  then  the 
current  kinematic  parameters  are  saved  as  the  correct  parameters.  If  not,  ZXSSQ  changes 
the  twenty-six  available  kinematic  parameters  and  repeats  the  process  until  the  convergence 
criteria  is  satisfied. 
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The  output  of  ID6_linsc  is  RESULT.DAT.  The  identified  kinematic 
parameters  will  be  stored  in  the  same  format  as  INPUT.DAT.  In  the  simulation  the 
identified  kinematic  parameters  will  be  the  parameters  of  INPUT.DAT  plus  dangle  for 
orientation  parameters  and  dlenth  for  length  parameters  plus  some  small  error  value  created 
by  the  addition  of  the  simulated  measurement  noise. 

C.     EXPERIMENTATION 

1.      The  Coordinate  Measuring  Machine  as  a  Linear  Slide 
a.      Construction 

The  coordinate  measuring  machine's  base,  which  supports  the  x-axis,  is 
mounted  on  an  incline,  as  illustrated  in  Figure  28.  The  incline  is  placed  on  the  table  on 
which  the  PUMA  is  mounted,  in  an  orientation  which  is  significantly  different  from  the  x- 
axis  of  the  PUMA.  This  position,  concurrent  with  the  inclined  mounting,  provides  for  the 
maximum  possible  joint  rotation  of  all  six  joints  of  the  PUMA. 
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Figure  28.   Coordinate  Measuring  Machine  as  Linear  Slide 

b.      Data  Acquisition   Using  the  Linear  Slide 

Data  acquisition  using  the  linear  slide  is  as  follows.  The  PUMA  is  fixed 
to  the  slide  using  a  plate  fitted  with  holes  to  facilitate  attachment  to  both  joint  six  and  the 
linear  slide.  The  mounting  plate  has  an  offset  which  allows  the  kinematic  parameters 
associated  with  the  sixth  transform  to  be  identified.  The  PUMA  is  placed  in  "Free"  mode, 
allowing  unimpeded  rotation  of  each  joint,  while  continually  being  supported  by  one 
member  of  the  measurement  team.  The  PUMA  is  quite  heavy  and  great  care  must  be  taken 
to  prevent  abrupt  movement  of  the  manipulator  arm  while  joint  six  is  attached  to  the  slide. 
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The  slide  is  moved  to  one  end  of  the  slide  and  the  x-axis  position  indicator  on  the  display 
unit  is  zeroed.  The  slide  should  be  positioned  at  the  lower  end  of  the  slide  when  being 
zeroed  in  order  to  have  positive  x-axis  position  indications  on  the  display  unit.  The  slide  is 
moved  up  the  scale  in  incremental  distances,  and  after  each  increment  the  six  joint  angle 
positions  of  the  PUMA  and  the  x  position  of  the  slide  is  input  using  program  ACDAT.  The 
position  which  has  been  input  is  stored  with  the  orientation  matrix,  which  will  not  change, 
regardless  of  the  position  of  the  slide. 

Two  operators  can  expect  to  measure  one  pose  in  one  minute.    One 
operator  is  utilized  to  move  and  support  the  PUMA  while  the  other  will  input  the  position 
of  the  slide  and  the  joint  angles  of  the  PUMA,  as  directed  by  program  ACDAT. 
c.      Identification  of  Actual  Kinematic  Parameters 

The  same  version  of  program  ID6_linsc  which  was  used  in  the 
identification  of  the  kinematic  parameters  in  the  simulation  phase  of  the  partial  pose 
calibration  is  used  in  the  identification  of  the  actual  kinematic  parameters.  Table  5  shows 
the  kinematic  parameters  identified  using  the  full  pose  calibration  procedure. 
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TABLE  5.     PARTIAL  POSE  IDENTIFIED  PARAMETERS 


Parameter 

Nominal  Value 

Identified  Value 

<l>b 

151.0 

150.4016 

eh 

-20.0 

-18.1370 

<Pb 

90.0 

90.1163 

Pxb 

-203.0 

-167.1160 

Pyb 

-254.0 

-382.9224 

Pzh 

457.2 

408.2330 

Bi 

0.0 

0.3920 

ai 

-90.0 

-89.9778 

662 

0.0 

-0.5600 

a^ 

431.85 

431.9559 

a2 

0.0 

-0.0648 

P2 

0.0 

0.0326 

66, 

0.0 

-1.0704 

d, 

149.1 

149.4573 

a, 

-20.3 

-19.3452 

a, 

90.0 

90.1312 

664 

0.0 

-0.8549 

d4 

433.0 

433.0048 

a4 

0.0 

0.6809 

a4 

-90.0 

-89.5235 

60s 

0.0 

2.3123 

ds 

0.0 

-0.3900 

as 

0.0 

-0.8460 

as 

90.0 

89.5360 

<k 

90.0 

78.6654 

e6 

0.0 

-0.1972 
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V.      DISCUSSION  OF  RESULTS 

A.     GENERAL  OBSERVATIONS 

Both  the  full  pose  and  partial  pose  calibration  methods  successfully  provided 
kinematic  parameters  which  could  be  used  in  a  control  program  to  use  the  PUMA  most 
accurately.  Each  simulation  predicted  convergence  to  a  unique  set  of  kinematic  parameters, 
thirty  for  the  full  pose  method  and  twenty-six  for  the  partial  pose  method.  Poses  measured 
in  the  laboratory  provided  the  data  which  proved  this  prediction. 

The  results  of  the  kinematic  parameter  identification  using  the  full  pose  calibration 
method  indicates  that  the  resulting  accuracy  of  the  manipulator  is  0.3  mm.  The 
experimentally  predicted  accuracy  was  considerably  larger  than  the  predicted  accuracy  from 
the  simulation,  which  was  approximately  0.15  mm.  This  is  most  likely  attributed  to  a 
higher  magnitude  of  noise  being  present  in  the  pose  measurements  than  predicted  in  the 
simulation.  It  may  be  concluded  that  using  this  type  of  calibration  method  will  improve  the 
accuracy  of  the  manipulator  to  about  the  same  value  of  the  manipulator  repeatability  [Ref. 
16]. 

The  results  of  the  kinematic  parameter  identification  using  the  partial  pose  calibration 
method  indicate  that  the  resulting  accuracy  of  the  manipulator  is  0.9  mm.  It  may  be 
concluded  that  using  this  type  of  calibration  method  will  provide  a  manipulator  which  is 
three  times  less  accurate  than  a  manipulator  calibrated  using  the  full  pose  method,  but  still 
more  accurate  than  an  uncalibrated  manipulator. 

The  method  to  determine  accuracy  of  the  partial  pose  calibration  method  is  as  follows. 
A  total  of  42  poses  were  measured,  21  in  lefty  configuration,  21  in  righty.  The  lefty  and 
righty  configuration  is  a  description  of  joint  l's  position.   If,  while  the  operator  is  facing 
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the  PUMA,  joint  1  is  rotated  such  that  the  axis  of  joint  one  points  to  the  operator's  right, 
the  PUMA  is  in  lefty  configuration.  The  opposite  holds  for  righty  configuration.  Figure 
29  shows  the  PUMA  in  lefty,  elbow  down  configuration.  Note  the  tooling  ball  end 
effector  attached  to  joint  six. 


Figure  29.  The  PUMA  560  in  Lefty,  Elbow  Down  Configuration 
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While  all  42  poses  were  used  in  the  identification  of  the  kinematic  parameters  shown 
in  Table  5,  to  determine  the  accuracy,  the  poses  were  divided.  The  first  ten  poses 
measured  in  the  lefty  and  righty  configuration  were  merged  to  form  a  set  of  twenty  poses 
which  were  used  in  program  ID6_LINSC  to  determine  a  set  of  identified  kinematic 
parameters,  an  output  file  KINPARAM.DAT.  The  last  ten  poses  measured  in  the  lefty  and 
righty  configuration  were  merged  into  a  data  file,  MEASURE.DAT,  to  act  as  the  measured 
poses.  The  program  F.FOR  was  executed,  calculating  the  predicted  poses  based  on  the 
actual  joint  angles  of  the  PUMA,  read  from  MEASURE.DAT,  and  the  identified  kinematic 
parameters,  read  from  KINPARAM.DAT.  The  result  is  two  sets  of  T6  matrices,  one  the 
actual  measured  T6  matrix,  the  other  the  predicted  T6  matrix.  Program  COMP.FOR 
calculates  the  RMS  position  difference  and  orientation  difference  between  the  two  sets  of 
T6  matrices. 

B.      COMPARATIVE  OBSERVATIONS 

Table  6  shows  the  identified  kinematic  parameters  for  the  PUMA  using  the  full  pose 
calibration  method  and  the  partial  pose  calibration  method.  A  comparison  of  the  two 
methods  of  calibration  described  in  this  thesis,  along  with  an  analysis  of  the  identified 
calibrated  parameters,  will  follow. 
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TABLE  6.     COMPARISON  BETWEEN  FULL  AND  PARTIAL  POSE 
IDENTIFIED  KINEMATIC  PARAMETERS 


Parameter 

Nominal  Value 

Identified  Value 
FULL  POSE 

Identified  Value 
PARTIAL  POSE 

ai 

0.0 

-0.04923 

0.3920 

ai 

-90.0 

-89.9977 

-89.9778 

oe? 

0.0 

-0.4888 

-0.5600 

a? 

431.9 

432.1216 

431.9559 

o> 

0.0 

-0.0303 

-0.0648 

02 

0.0 

-0.01515 

0.0326 

60^ 

0.0 

-1.2069 

-1.0704 

ch 

149.1 

149.1455 

149.4573 

ai 

-20.3 

-19.2270 

-19.3452 

ct^ 

90.0 

90.0512 

90.1312 

664 

0.0 

-0.9144 

-0.8549 

d4 

433.0 

432.8899 

433.0048 

aa 

0.0 

0.0040 

0.6809 

a4 

-90.0 

-89.9909 

-89.5235 

66, 

0.0 

2.2364 

2.3123 

ds 

0.0 

-0.6629 

-0.3900 

as 

0.0 

-0.0258 

-0.8460 

as 

90.0 

89.9345 

89.5360 

Most  robot  manipulator  arms  will  be  used  in  an  industrial  environment.  While  it  is 
important  that  the  manipulator  be  properly  calibrated,  the  amount  of  time  the  manipulator  is 
unavailable  for  operation  due  to  the  calibration  process  being  performed  is  significant.  The 
experiments  performed  clearly  have  shown  the  full  pose  method  takes  considerably  more 
time  to  accomplish  than  the  partial  pose  method.  The  full  pose  method  takes  approximately 
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ten  minutes  to  make  one  pose  measurement,  while  the  partial  pose  measurement  takes  only 
one  minute  to  measure  one  pose.  The  amount  of  time  required  could  be  cut  dramatically  if 
an  interface  between  the  PUMA  control  computer  and  the  data  acquisition  computer  were 
built.  This  interface  would  eliminate  the  need  for  manual  entry  of  the  six  joint  angles  for 
each  pose.  The  relative  difference  in  the  amount  of  time  required  to  measure  one  pose 
would  still  be  present. 

The  full  pose  calibration  method  provides  for  a  more  accurate  calibration  of  the 
manipulator.  This  fact  arises  from  two  considerations.  First,  more  of  the  kinematic 
parameters  of  the  manipulator  can  be  identified  using  the  full  pose  method,  thirty,  than  the 
partial  pose  method,  twenty-six.  A  thirty  parameter  model  is  logically  a  more  detailed 
model  of  the  manipulator  than  a  twenty-six  parameter  model.  Second,  the  full  pose 
calibration  uses  a  working  volume  of  the  manipulator  which  is  significantly  larger  than  the 
working  volume  used  in  the  partial  pose  method.  In  actuality,  the  area  used  in  the  full  pose 
calibration  is  indeed  a  volume.  The  area  used  in  the  partial  pose  model  is  comprised  of 
only  a  line  which  is  slightly  longer  than  900  mm.  This  thesis  did  not  examine  the  validity 
of  the  calibration  of  the  PUMA  once  the  PUMA  operates  outside  the  calibration  volume.  It 
seems  logical  that  if  the  tool  of  the  PUMA  was  placed  a  considerable  distance  from  the 
region  in  which  it  was  calibrated,  the  calibration  becomes  invalid. 

Another  difference  between  the  full  and  partial  pose  methods  is  the  association 
between  the  number  of  poses  required  for  a  calibration.  Each  pose  measured  using  the  full 
pose  calibration  provides  six  units  of  information:  the  position,  (x6-  y6^  z6)>  °f  the  origin 
of  frame  6,  and  the  orientation  of  frame  6,  the  direction  cosines  of  each  coordinate  axis. 
The  full  pose  calibration  determined  thirty  kinematic  parameters.  In  the  complete  absence 
of  noise  five  poses  would  be  required  to  identify  the  kinematic  parameters,  since  thirty 
unknowns  require  thirty  units  of  information  for  a  unique  solution.   Each  pose  measured 
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using  the  partial  pose  calibration  provides  two  units  of  information,  since  the  frame  which . 
the  pose  is  defined  with  respect  to,  is  not  known.  Four  units  of  information  are  required  to 
define  the  frame,  the  sum  of  which  is  six  units  of  information,  as  is  available  in  the  full 
pose  calibration.  In  the  complete  absence  of  noise  fourteen  poses  would  be  required  to 
identify  the  kinematic  parameters,  since  twenty-six  unknowns  require  twenty-six  units  of 
information  for  a  unique  solution. 

The  full  pose  calibration  showed  that  it  is  very  important  to  exercise  all  six  of  the 
joints  as  much  as  possible  during  the  pose  measurement  phase.  This  requirement  arises 
due  to  the  presence  of  measurement  noise  [Ref.  17].  The  simulation's  level  of 
measurement  noise  was  not  as  large  as  the  noise  determined  to  be  present  in  the  actual 
measurement  phase.  This  allowed  for  a  unique  solution  in  the  simulation  without  large 
joint  rotations.  The  measured  pose  calibration  process  showed  the  increased  amount  of 
noise  being  present,  and  it  was  determined  that  maximizing  the  joint  rotation  alleviated  the 
presence  of  the  measurement  noise  in  the  coordinate  measuring  machine  and  in  the 
encoders  within  the  joints  of  the  PUMA.  The  PUMA  was  exercised  by  taking  a  series  of 
poses  which  would  transcribe  a  cube  in  front  of  the  PUMA,  in  elbow  up  and  elbow  down 
(joint  3  is  the  elbow),  and  in  lefty  and  righty  configurations. 

The  problem  of  exercising  the  joints  resurfaced  in  the  partial  pose  calibration.  The 
slide's  length  and  position  relative  to  the  PUMA  prevented  adequate  exercising  of  the 
joints.  If  poses  which  were  taken  in  only  one  configuration,  i.e.  lefty  or  righty,  were  used 
in  a  calibration  attempt  using  ID6_LINSC,  the  identified  kinematic  parameters  were  found 
to  be  significantly  different  from  those  identified  using  the  full  pose  method,  and  were 
disregarded  as  inaccurate.  The  presence  of  noise  arising  from  the  slide's  flexure  on  its' 
mount  dominated  in  this  case.  The  first  slide  used  in  the  partial  pose  calibration  process 
had  dramatic  flexure,  and  was  discarded  in  preference  of  the  more  stiff  coordinate 
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measurement  machine  x-axis  slide.  Even  with  the  reasonably  stiff  coordinate  measuring 
machine  it  was  shown  that  regardless  of  how  many  poses  were  measured  in  one 
configuration,  an  accurate  calibration  was  not  possible.  To  conduct  an  accurate  calibration, 
a  series  of  poses  were  measured  along  the  length  of  the  slide  and  then  the  PUMA  was 
disconnected  from  the  slide.  Joint  1  was  rotated  such  that  the  configuration  was  reversed 
from  the  lefty  configuration  to  the  righty  configuration,  the  tool  frame  was  reattached  to  the 
slide,  and  another  series  of  poses  was  measured  along  the  length  of  the  slide.  It  was  not 
possible  to  obtain  poses  in  both  the  elbow  up  an  elbow  down  configuration  due  to  the 
proximity  of  the  PUMA  to  the  slide.  All  of  the  poses  were  taken  in  the  elbow  up 
configuration.  Measuring  poses  in  both  the  lefty  and  righty  configuration  countered  the 
presence  of  the  measurement  noise,  but  the  resultant  accuracy  of  the  calibration  still  did  not 
match  that  of  the  full  pose  calibration. 

The  orientation  of  the  slide  presented  a  problem  as  well.  Two  calibration  attempts 
were  attempted  with  the  slide  laying  flat  on  the  table  on  which  the  PUMA  is  mounted.  It 
was  found  that  the  orientation  of  the  slide  prevented  the  joint  rotation  necessary  to 
overcome  the  measurement  noise  present.  The  next  attempt  placed  the  slide  on  an  incline  of 
about  thirty  degrees.  The  slide/incline  apparatus  was  first  oriented  nominally  in  line  with 
the  x-axis  of  the  PUMA.  Again,  adequate  joint  rotation  was  not  possible.  The  difficult 
joints  to  move  were  the  wrist  joints,  joints  4  through  6.  Also,  there  was  a  problem  with  the 
tool  frame  entering  areas  of  singularity.  The  area  is  said  to  be  singular  if  the  tool  frame  can 
be  restrained  in  one  position  while  a  joint  is  moved  freely.  Simply  stated,  this  means  a 
singular  pose  can  be  defined  with  several  different  sets  of  joint  angles.  To  remove  the 
problem  of  singularity  and  to  achieve  the  required  joint  rotation,  the  slide/incline  apparatus 
was  placed  at  an  angle  from  the  x-axis  of  the  PUMA.   Using  this  orientation  and  the  pose 
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measurement    procedure    described    in    the    previous    paragraph,    the    calibration    was 
completed. 
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VI.    CONCLUSIONS 

The  results  of  the  research  reported  in  this  thesis  provide  the  following  conclusions: 

1.  It  is  possible  to  improve  the  accuracy  of  a  PUMA  560  robot  manipulator  to 
significantly  less  than  the  current  possible  accuracy  of  10.0  mm,  and  equal  to  the 
repeatability,  0.3  mm. 

2.  Full  pose  calibration  of  the  PUMA  560  is  possible  and  will  identify  thirty 
kinematic  parameters,  producing  an  accuracy  of  0.3  mm. 

3.  Partial  pose  calibration  of  the  PUMA  560  is  possible  and  will  identify  twenty- 
six  kinematic  parameters,  producing  an  accuracy  of  0.9  mm. 

4.  Obtaining  the  maximum  possible  joint  rotation  of  all  six  joints  is  required  for 
robust  convergence  to  the  identified  kinematic  parameters  of  the  manipulator. 

5.  Measuring  poses  in  lefty,  righty,  elbow  up,  and  elbow  down  configuration 
facilitates  maximum  joint  rotation  and  corresponding  robust  convergence. 

6.  The  partial  pose  measurement  phase  showed  that  the  orientation  of  the  slide 
relative  to  the  PUMA  is  of  paramount  importance  in  the  identification  of  the  kinematic 
parameters. 

7.  More  information  per  pose  is  determined  using  the  full  pose  calibration,  but  the 
excessive  time  required,  ten  minutes  per  pose,  detracts  from  the  efficiency  of  the  method. 

8.  To  use  the  partial  pose  method  of  calibration  requires  that  the  manipulator  have 
a  "Free"  mode,  in  which  each  joint  can  be  physically  rotated. 

9.  The  "Delta"  matrix,  showed  in  equation  35  should  not  be  used  as  the  functions 
to  be  minimized  in  the  calibration  procedure.  Each  element  of  the  T6  matrix  should  be  used 
as  functions  to  be  minimized. 
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APPENDIX  A.  PROGRAM   BALL1 


Program  BALL 

c       This  program  takes  four  sets  of  Cartesian  coordinates 

c       assumed  to  be  from  a  CMM  touching  the  surface  of  a  precision  tooling 

c      ball,  and  finds  the  center  of  the  ball. 

integer  n , m , ns  ig , maxf n , iopt , i , infer , ier , ix j  ac 
real*8  parm(4) ,x(3) ,f (4) ,xjac(4,3) ,work(29) ,eps, delta, 
&         xjtj(6),y(4,3) 
external  res id 
common  y 

m=4 

n=3 

ixjac=4 

nsig=3 

eps=0.0 

delta=0.0 

maxfn=500 

iopt=l 

c  get  touch  data 

1000   do  i=l,4 

write(6,*) 'Move  the  CMM  and  type  in  data  for  point  #',i 

read  (5,*)  y(i, 1 ) ,y(i,2 ) ,y(i, 3 ) 

enddo 

c  set  up  start  point  for  identification 

x(l)=y(l,l) 
x(2)=y(l,2) 
x(3)=y(l,3) 

call  zxssq (resid,m,n,nsig,eps, delta, maxf n, iopt, parm,x,ssq,f , 
&  xjac,ixjac,xjt j ,work, infer, ier ) 

write(6,800)  'center  (x,y,z)  is: ' ,x(l ) ,x(2) ,x(3 ) 
write (6,*)  'residual  error  is:',  ssq 
800   format(lx,a,3f9.3) 

end 


subroutine  resid(x,m,n,f ) 

c       This  subroutine  calculates  the  residual  functions  used  by  the 
c      IMSL  routine  ZXSSQ,  called  from  subroutine  BALL. 

integer  m,n 

real* 8  x(n),f(m),y(4,3) ,res,re,r0 
common  y 
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r0=7.85 
do   i=l,4 

res=(x(l)-y(i,l) ) **2+(x(2 )-y(i, 2 ) ) **2+(x(3 )-y(i, 3 ) ) **2 

re=dsqrt ( res ) 

f (i)=abs(re-rO) 
enddo 

sum=0.0 
do  i-1,4 

8um=suirri-f  (i) 
enddo 

write (6,*)    sum 

return 

end 
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APPENDIX  B    PROGRAM  CMMPOSE 


Program  CMMPOSE 

c  This  program  accepts  coordinate  data  from  the  CMM,  together 

c  with  the  tooling  ball  i.d.  and  generates  a  4x4  pose  matrix 

c  corresponding  to  the  pose.  Four  measurements  on  each  of  three 

c  balls  is  required. 

parameter  ( lda=4 , ldainv=4 , n=4 , ld3=3 ) 

real*8  x(3,3),z(3),p(5,3),pa(3),pb(3),pc(3),pd(3),p4(4,4), 
&         pd4 (4,4 ) ,pinv(4, 4 ) ,t(4,4 ) , sum, val,xl,yl, zl, 
&         x2,y2,z2 

integer  id ( 3 ) , flag 

character  reply 

external  wrrrn 

c  Precalibration  data  for  the  tool  ( in  column  order ) 

data  p/0. 0,50. 740, 0.0, -50. 913, 0.0, 
&  0.0,0.0,50.703,0.0,-50.988, 
&         51.111,0.0,0.0,0.0,0.0/ 

c  open  data  file 

open ( 9 , name= ' posex . dat ' , status= ' new ' ) 

c  gather  data  from  three  balls 

1000   do  j=l,3 

write (6,*)  'Input  ball  reference  number' 
read(5,*)  id(j) 
call  ball(z) 

do  k=l,3 

x(j,k)=z(k) 

enddo 
enddo 

c  synthesize  the  fourth  ball  -  measured 

do  j=l,3 
pa( j)=x(l, j) 
pb(j)=x(2,j) 
pc(j)=x(3, j) 
enddo 

xl=(pb(l)-pa(l) )/70.71 
yl=(pb(2)-pa(2))/70.71 
zl=(pb(3)-pa(3) )/70.71 

x2=(pc(l)-pa(l) )/70.71 
y2=(pc(2)-pa(2))/70.71 
z2=(pc(3)-pa(3) )/70.71 
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pd(l)=(yl*z2-y2*zl)*70.71+pa(l) 
pd(2)=(x2*zl-xl*z2)*70.71+pa(2) 

pd(3)=(xl*y2-x2*yl)*70.71+pa(3) 

c  compose  the  PD4  matrix 

do  i-1,3 

pd4(i,l)=pa(i) 
pd4(i,2)=pb(i) 
pd4(i,3)=pc(i) 
pd4(i,4)=pd(i) 
enddo 

do  i=l,4 

pd4(4,i)=1.0 
enddo 

c  synthesize  the  fourth  ball  -  tool 

do  j-1,3 
pa(j)-p(id(l),j) 

pb(j)=p(id(2),j) 

pc(j)=p(id(3),j) 
enddo 

xl=(pb(l)-pa(l) )/70.71 
yl=(pb(2)-pa(2))/70.71 
zl=(pb(3)-pa(3) )/70.71 

x2=(pc(l)-pa(l) i/70.71 
y2=(pc(2)-pa(2))/70.71 
z2=(pc(3)-pa(3) )/70.71 

pd(l)=(yl*z2-y2*zl)*70.71+pa(l) 
pd(2)=(x2*zl-xl*z2)*70.71+pa(2) 
pd(3)=(xl*y2-x2*yl)*70.71+pa(3) 

c  compose  the  P4  matrix 

do  i-1,3 

p4(i,l)=pa(i) 
p4(i,2)=pb(i) 
p4(i,3)=pc(i) 
p4(i,4)=pd(i) 
enddo 

do  i-1,4 
p4(4,i)-1.0 

enddo 

c  calculate  the  T  matrix 

call  dlinrg(n,p4,lda,pinv,ldainv) 

call  matmulc ( t , pd4 , pinv ) 

call  dwrrrn ( ' T ' , n , n , t , Ida  i  nv , 0 ) 

c  orthogonality  and  size  check 

flag=0 
8um=0.0 

do  j-1,3 
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8um=sum+t  (  j  ,  1 )  *t  (  j  ,  2  ) 

enddo 

val=dabs(sum) 

write ( 6 , * ) ' val= ' , val 

if (val.gt.0.01)  flag=l 

do  j=l,3 

sum=8um+t  (  j ,  2  )  *t  ( j  ,  3 ) 

enddo 

val=dabs  ( sum ) 

write ( 6 , * ) ' val= ' , val 

if (val.gt.0.01)  flag=l 

do  j=l,3 

sum=suin+t  ( j ,  1 )  *  t  ( j ,  3 ) 

enddo 

val=dabs(sum) 

write ( 6 , * ) ' val= ' , val 

if (val.gt.0.01)  flag=l 


do  i=l,3 

sum=0 . 0 


do  j=l,3 

sum=sum+t ( j , i ) *t ( j , i ) 

enddo 

val=dabs ( sum-1 . 00000 ) 

write ( 6 , * ) ' val= ' , val 

if (val.gt.0.01)  flag=l 


enddo 


if (flag.eq.l )  then 

write (6,*)  'orthogonality  test:  FAIL' 
else 

write (6,*)  'orthogonality  test:  PASS' 
endif 

c  save  data  to  file 

do  i=l,4 

write(9,700)  t(i,l) ,t(i,2) ,t(i,3) ,t(i,4) 

enddo 
700   format(4el7.8) 
write (9,*) 

c  reminder  to  get  joint  angles 

write (6,*) "Type  WHERE  on  the  PUMA  console' 

c  need  more  data  ? 

1020   write(6,*) 'More  data. .. (y=yes,  n=no) ' 
read  (5, '(a)')  reply 
if  (reply. eq. 'y' )  goto  1000 
if  (reply. eq. 'n' )  goto  1010 
goto  1020 


1010  close  (9) 
close(10) 
end 
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subroutine  BALL(x) 

c      This  program  takes  three  or  four  sets  of  Cartesian  coordinates 

c      assumed  to  be  from  a  CMM  touching  the  surface  of  a  precision  tooling 

c      ball,  and  finds  the  center  of  the  ball. 

external  res id 

integer  n , m , ns ig , maxf n , iopt , i , infer , ier , ix j  ac 
real*8  parm(4) ,x(3) ,f (4) ,xjac(4,3) , work( 29 ) ,eps, delta, 
&         xjtj(6),y(4,3) 
character  reply 
common  y 

open( 10,name='test.dat ' ,status='old'  ) 

m=4 
n=3 
ixjac=4 

nsig=3 
eps=0 . 0 
delta=0 . 0 
maxfn=500 
iopt=l 

c  get  touch  data 

1020   do  i=l,4 

write(6,*) 'Move  the  CMM  and  type  in  data  for  point  #',i 
c      read  (10,*)y(i,l) ,y(i,2 ) ,y(i,3 ) 
read  (5,*)  y(i, 1 ) ,y(i,2 ) ,y(i, 3 ) 
write(10,*)y(i,l),y(i,2),y(i,3) 
enddo 

c  set  up  start  point  for  identification 

x(l)=y(l,l) 
x(2)=y(l,2) 
x(3)=y(l,3) 

call  zxssq (resid,m,n,nsig,eps, delta, maxf n, iopt, parm,x,ssq,f, 
&  xjac,ixjac,xjtj , work, infer, ier) 

write(6,800)  'center  (x,y,z)  is: ' ,x(l) ,x(2) ,x(3) 
write (6,*)  'residual  error  is:',  ssq 
800   format(lx,a,3f9.3) 

1000   write(6,*)  'Keep  this  data  (y=yes,  n=no) ' 
read  (5, ' (a) ' )  reply 
if  (reply. eq. 'y' )  goto  1010 
if  (reply. eq. 'n' )  goto  1020 
goto  1000 

1010   return 
end 


subroutine  resid(x,m,n,f ) 
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c       This  subroutine  calculates  the  residual  functions  used  by  the 
c       IMSL  routine  ZXSSQ,  called  from  subroutine  BALL. 

integer  m,n 

real* 8  x(n),f(m),y(4,3) ,res,re,rO 

common  y 

r0=7.85 
do  i=l,4 

res=(x(l)-y(i,l))**2+(x(2)-y(i,2))**2+(x(3)-y(i,3))**2 

re=dsqrt ( res ) 

f (i)-abs(re-rO) 
enddo 

sum=0.0 
do  i=l,4 

sum=sum+f (i) 
enddo 

return 

end 
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APPENDIX  C    PROGRAM  JOINT 


PROGRAM  JOINT 

c  This  program  generates  the  six  joint  angles  for  the 

c  PUMA  manipulator  arm  for  the  simulation  of  the  PUMA's 

c  calibration  using  the  coordinate  measurement  machine, 

c  The  six  joint  angles  are  generated  using  a  random  number 

c  generator . 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 

PARAMETER  (MAXNOBS=360 ) 

REAL  Q(MAXN0BS,6) ,  QMIN(6),  QMAX(6) 

COMMON  /CI/  Q,  QMAX,  QMIN 

c   In  this  section  the  working  volume  of  the  PUMA  can  be  selected, 
c   The  working  volume  can  be  full,  half,  or  one  quarter,  based  on 
c   extent  of  the  allowed  joint  movement. 

DATA  QMIN/  -180.0,  -180.0,  -180.0,  -180.0,  -180.0,  -180.0  / 
DATA  QMAX/   180.0,  180.0,  180.0,  180.0,  180.0,  180.0  / 
WRITE  (6,*)  'Volume  is  FULL' 

c       DATA  QMIN/  -90.0,  -90.0,  -90.0,  -90.0,  -90.0,  -90.0  / 
c       DATA  QMAX/  90.0,  90.0,  90.0,  90.0,  90.0,  90.0  / 
c        WRITE  (6,*)  'Volume  is  HALF' 

c       DATA  QMIN/  -45.0,  -45.0,  -45.0,  -45.0,  -45.0,  -45.0  / 
c       DATA  QMAX/  45.0,  45.0,  45.0,  45.0,  45.0,  45.0  / 
c        WRITE  (6,*)  'Volume  is  QUARTER' 

c   Open  the  output  data  file,  PUMA- VAR . DAT . 

OPEN  (8,  NAME=' PUMA- VAR. DAT",  STATUS= ' NEW ' ) 

c   Input  the  number  of  observations  from  the  nominal  kinematic 
c   data  file,  INPUT.DAT. 

OPEN  (9,  NAME=' INPUT. DAT',  STATUS= ' OLD ' ) 

DO  1=1,10 

READ  (9,*) 
ENDDO 

READ  (9,*)  NOBS 

CLOSE  (9) 

c   Call  the  random  number  generation  routine  to  obtain  a  set 
c   of  six  random  joint  angles. 

CALL  MSPREAD  (NOBS) 

c   Save  the  joint  variable  data  in  the  output  file,  PUMA- VAR . DAT . 
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DO  II  =  1,  NOBS 

WRITE  (8,*)  Q(II,1),Q(II,2),Q(II,3),Q(II,4),C(II,5),Q(II,6) 
ENDDO 

CLOSE  (8) 

STOP 

END 

c   "SUBROUTINES** 

c   "SUBROUTINE  1** 

SUBROUTINE  MSPREAD  (NOBS) 

c   This  subroutine  generates  the  six  required  joint  angles 
c   by  the  Monte  Carlo  method.  The  six  joint  angles  are 
c   generated  by  using  six  independently  randan  numbers. 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 

PARAMETER  (MAXNOBS=360 ) 

REAL  Q ( MAXNOBS ,  6 )  ,  OMIN ( 6 ) ,  OMAX ( 6 ) ,  MAGQ ( 6 ) ,  NUM 

INTEGER* 4  I SEED 

COMMON  /CI/  Q,  QMAX,  QMIN 

c   Obtaining  the  random  number  seed. 

WRITE  (6,*)  'Type  in  a  6-digit  random  number  seed' 
READ   (5,*)  ISEED 

c   Calculating  the  scaling  factor  for  each  joint  angle. 

DO  I  =  1,  6 

MAGQ(I)  =  QMAX ( I ) -OMIN ( I ) 
ENDDO 

c   Generating  the  six  joint  angles. 

DO  J  =  1,  NOBS 
DO  I  =  1,  6 
CALL  RANDOM  (ISEED, NUM) 
Q(J,I)  =  OMIN(I)  +  MAGQ(I)  *  NUM 
ENDDO 
ENDDO 

RETURN 

END 


c   "SUBROUTINE  2** 

SUBROUTINE  RANDOM  (X,Z) 

REAL  FM,  FX,  Z 
INTEGER  A,  X,  I,  M 
DATA  1/1/ 
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1000 


IF  (  I  .EQ. 
1=0 

M=  2  **  20 
FM=  M 
A=  2**10  + 

0  ) 

3 

GO 

TO 

1000 

X=  MOD( 
FX=  X 
Z=  FX/ 

A*X 

FM 

,M) 

RETURN 

END 
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APPENDIX  D    PROGRAM   POSE 


PROGRAM  POSE 

c  This  program  generates  a  T6  matrix  for  each  set  of  joint 

c  angles  which  were  generated  using  the  program  JOINT.   The  T6 

c  matrix  is  calculated  by  performing  a  forward  kinematic  solution 

c  using  the  six  joint  angles  which  are  read  from  PUMA-VAR.DAT 

c  and  the  nominal  kinematic  data  which  are  read  from  INPUT.DAT. 

INTEGER* 4  ISEED 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS,  N 

REAL*8  RNX,  RNY,  RNZ,  MAGNX,  MAGN1 ,  DANGLE,  DLENTH 
REAL*8  RN1,  RN2 ,  RN3 ,  RN4,  RN5,  RN6,  PI 

PARAMETER  (PI=3 . 141592653589793 ) 
PARAMETER  (MAXNOBS=360 ) 

REAL*8  FIO,  SIO,  THO,  PXO,  PYO,  PZO 

REAL*8  DTI,  DT2 ,  DT3,  DT4,  DT5 

REAL*8  DD1,  DD2 ,  DD3 ,  DD4 ,  DD5 

REAL*8  AA1,  AA2 ,  AA3 ,  AA4 ,  AA5 

REAL*8  AL1,  AL2 ,  AL3 ,  AL4 ,  AL5 

REAL*8  BL1,  BL2 ,  BL3,  BL4 ,  BL5 

REAL*8  FI6,  TH6,  SI6,  PX6,  PY6 ,  PZ6,  DF6 

REAL* 8  THETA1,  THETA2 ,  THETA3 ,  THETA4 ,  THETA5 ,  THETA6 

REAL*8  TH1,  TH2 ,  TH3,  TH4 ,  TH5 

REAL*8  TO (4,4),  Tl(4,4),  T2(4,4),  T3(4,4) 

REAL*8  T4(4,4),  T5 { 4 , 4 ) ,  T6(4,4),  TRPY(4,4),  TXYZ ( 4 , 4 ) 

REAL*8  TIMAT(4,4),  T(4,4) 

c   Initializing  the  TIMAT  matrix  to  an  identity  matrix. 

DATA  T IMAT / 1 , 0,0, 0,0, 1,0, 0,0, 0,1, 0,0, 0,0,1/ 

c   Obtain  a  random  number  seed.  This  random  number  will  be  used 
c   later  in  the  program  to  generate  random  noise  which  will  model 
c   an  actual  measurement  using  the  coordinate  measurement  machine. 

WRITE  (6,*)  'Type  in  a  6-digit  random  number  seed. ' 
READ   (5,*)  ISEED 

c  Open  the  two  input  files,  PUMA-VAR.DAT  and  INPUT.DAT,  and 

c  the  output  data  file,  PUMA-POS . DAT ,  which  will  contain  the 

c  six  previously  determined  joint  angles  and  the  corresponding 

c  T6  matrix  calculated  in  this  program. 

OPEN  (8,  NAME=' PUMA-VAR.DAT' ,  STATUS= ' OLD ' ) 
OPEN  (9,  NAME= 'PUMA-POS. DAT ' ,  STATUS= ' NEW ' ) 
OPEN  (10,NAME=' INPUT. DAT',  STATUS= ' OLD ' ) 

c   Reading  nominal  kinematic  parameters  from  INPUT.DAT. 

read  (10,*) 
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read 

(10,*) 

read 

(10,*) 

read 

(10,*) 

read 

(10,*) 

read 

(10,*) 

read 

(10,*) 

read 

(10,*) 

read 

(10,*) 

read 

(10,*) 

read 

(10,*) 

f iO , thO , siO , pxO , pyO , pzO 

dtl,ddl,aal,all,bll 

dt2,dd2,aa2,al2,bl2 

dt3,dd3,aa3,al3,bl3 

dt4,dd4,aa4,al4,bl4 

dt5,dd5,aa5,al5,bl5 

df 6 , th6 , si6 , px6 , py6 , pz6 

nobs ,n, dangle, dlenth, magnx,magnl 

c   Adding  encoder  offsets  and  setting  link  parameters,  creating 
c   a  PUMA  manipulator  different  from  the  manipulator  represented 
c   by  the  nominal  kinematic  parameters . 

dtl  =  dtl 

dt2  =  dt2  +  dangle 
dt3  =  dt3  +  dangle 
dt4  =  dt4  +  dangle 
dt5  =  dt5  +  dangle 


fiO  = 

fiO 

+ 

dangle 

thO  = 

thO 

+ 

dangle 

siO  = 

siO 

+ 

dangle 

pxO  = 

pxO 

+ 

dlenth 

pyo  = 

pyO 

+ 

dlenth 

pzO  = 

pzO 

+ 

dlenth 

all  = 

all 

+ 

dangle 

al2  = 

al2 

+ 

dangle 

al3  - 

al3 

+ 

dangle 

al4  - 

al4 

+ 

dangle 

al5  = 

al5 

+ 

dangle 

aal  = 

aal 

+ 

dlenth 

aa2  = 

aal 

+ 

dlenth 

aa3  = 

aa3 

+ 

dlenth 

aa4  = 

aa4 

+ 

dlenth 

aa5  = 

aa5 

+ 

dlenth 

ddl  = 

ddl 

dd2  = 

0. 

) 

1  defined 

dd3  = 

dd3 

+ 

dlenth 

dd4  - 

dd4 

+ 

dlenth 

dd5  = 

dd5 

+ 

dlenth 

bll  = 

bll 

!  defined 

bl2  = 

bl2 

+ 

dangle 

bl3  = 

bl3 

1  defined 

bl4  = 

bl4 

!  defined 

bl5  = 

bl5 

I  defined 

df6  - 

df6 

+ 

dangle 

th.6  = 

th6 

+ 

dangle 

si6  = 

si6 

+ 

dangle 

px6  = 

px6 

+ 

dlenth 

py6  = 

py6 

+ 

dlenth 

pz6  = 

pz6 

+ 

dlenth 

c   Loop  through  the  program,  generating  a  T6  matrix  for  each 

c   set  of  6  joint  angles.  The  number  of  loops  will  be  reflected 
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c   by  the  entry  "nobs"  in  the  nominal  kinematic  parameter  file, 

c   INPUT . DAT . 

DO  I  =  1,  NOBS 
c   Initializing  the  T  matrix  to  an  identity  matrix. 

DO  J=l,4 

DO  K=l,4 
T(J,K)  =  TIMAT(J,K) 

ENDDO 
ENDDO 

c   Read  the  sets  of  six  joint  angles  from  the  data  file  PUMA-VAR . DAT . 

READ  (8,*)  THETA1,  THETA2 ,  THETA3,  THETA4,  THETA5,  THETA6 

TH1  =  DTI  +  THETA1 
TH2  =  DT2  +  THETA2 
TH3  =  DT3  +  THETA3 
TH4  =  DT4  +  THETA4 
TH5  =  DT5  +  THETA5 
FI6  =  DF6  +  THETA6 

c   Computing  the  six  individual  T  matrices,  Tl  thru  T6. 

CALL  T3RPY  (  FIO,  THO,  SIO,  TRPY  ) 
CALL  T3XYZ  (  PXO ,  PYO,  PZO,  TXYZ  ) 
CALL  MATMULC  (  TO,  TRPY,  TXYZ  ) 

CALL  TRANSFORM  (  AL1,  AA1 ,  DD1,  TH1 ,  BL1,  Tl 

CALL  TRANSFORM  (  AL2 ,  AA2 ,  DD2 ,  TH2 ,  BL2 ,  T2 

CALL  TRANSFORM  (  AL3,  AA3 ,  DD3,  TH3,  BL3,  T3 

CALL  TRANSFORM  (  AL4 ,  AA4 ,  DD4,  TH4,  BL4 ,  T4 

CALL  TRANSFORM  (  AL5,  AA5,  DD5,  TH5,  BL5,  T5 

CALL  T3RPY  (  FI6,  TH6,  SI6,  TRPY  ) 
CALL  T3XYZ  (  PX6,  PY6,  PZ6,  TXYZ  ) 
CALL  MATMULC  (  T6 ,  TRPY,  TXYZ  ) 

c   Computing  the  overall  transformation,  T. 

CALL  MATMULA  (  T,  TO 

CALL  MATMULA  (  T,  Tl 

CALL  MATMULA  (  T,  T2 

CALL  MATMULA  (  T,  T3 

CALL  MATMULA  (  T,  T4 

CALL  MATMULA  (  T,  T5 

CALL  MATMULA  f  T,  T6 

c  Generating  the  random  noise.  This  noise  will  be  added  to 

c  the  theoretical  measurement  data  and  the  encoder  readings 

c  to  accurately  simulate  the  actual  measurements  which  will 

c  be  made  in  the  lab. 

CALL  RANDOM (I SEED, RNX) 
CALL  RANDOM (I SEED, RNY ) 
CALL  RANDOM(ISEED,RNZ) 

CALL  RANDOM (I SEED, RN1 ) 
CALL  RANDOM (I SEED, RN2) 
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CALL  RANDOM (I SEED, RN3) 
CALL  RANDOM (I SEED, RN4) 
CALL  RANDOM (I SEED, RN5) 
CALL  RANDOM (I SEED, RN6) 


RNX 

= 

MAGNX*( 

2 

0*RNX 

- 

1 

0  ) 

RNY 

= 

MAGNX*( 

2 

0*RNY 

- 

1 

0  ) 

RNZ 

= 

MAGNX*( 

2 

.0*RNZ 

- 

1 

0  ) 

RN1 

= 

MAGN1*( 

2 

0*RN1 

_ 

1 

0  ) 

RN2 

= 

MAGN1*( 

2 

0*RN2 

- 

1 

0  ) 

RN3 

= 

MAGN1*( 

2 

0*RN3 

- 

1 

0  ) 

RN4 

= 

MAGN1*( 

2 

0*RN4 

- 

1 

0  ) 

RN5 

= 

MAGN1*( 

2 

0*RN5 

- 

1 

0  ) 

RN6 

= 

MAGN1*( 

2 

0*RN6 

- 

1 

0  ) 

c   Adding  the  noise  to  measurement  and  encoder  readings . 


T(l,4) 

= 

T(l,4) 

+  RNX 

T(2,4) 

= 

T(2,4) 

+  RNY 

T(3,4) 

= 

T(3,4) 

+  RNZ 

THETA1 

= 

THETAI 

+RN1 

THETA2 

= 

THETA2 

+RN2 

THETA3 

= 

THETA3 

+RN3 

THETA4 

= 

THETA4 

+RN4 

THETA5 

= 

THETA5 

+RN5 

THETA6 

= 

THETA6 

+RN6 

c   Storing  the  manipulator  joint  angles,  calculated  in  the 

c   program  JOINT  and  the  theoretical  measured  tool  pose, 

c   calculated  in  this  program,  in  the  data  file  PUMA- POS . DAT . 

WRITE  (9,991)  THETAI,  THETA2 ,  THETA3,  THETA4,  THETA5,  THETA6 

WRITE  (9,992)  T(l,l),  T(l,2),  T(l,3),  T(l,4) 

WRITE  (9,992)  T(2,l),  T(2,2),  T(2,3),  T(2,4) 

WRITE  (9,992)  T(3,l),  T(3,2),  T(3,3),  T(3,4) 
WRITE  (9,*) 

c   Format  below  decides  the  digits  of  accuracy  of  the 
c   simulation  data. 


991  FORMAT  (  6F12.6  ) 

992  FORMAT  (  3F16.10,  F12.5  ) 

ENDDO 


Uoint  vector  data 
! Measurement  data 


WRITE  (6,*)  'Data  stored  in  F12.5,  F12.4  format' 


CLOSE  (8) 
CLOSE  (9) 

STOP 

END 

C  "SUBROUTINES** 
C  **SUBROUTINE  1** 
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1000 


SUBROUTINE  RANDOM  (X,Z) 

REAL  FM,  FX,  Z 
INTEGER  A,  X,  I,  M 

DATA  1/1/ 

IF  (  I  .EQ.  0  )  GO  TO  1000 
1=0 

M=  2  **  20 
FM=  M 
A=  2**10  +  3 


X=  MOD(    A*X 

,M) 

FX=   X 

Z=  FX/   FM 

RETURN 
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APPENDIX   E     PROGRAM   ID6 


PROGRAM  ID6 

c  This  is  the  program  which  identifies  the  kinematic  parameters 
c  of  the  simulated  PUMA  560  manipulator  using  the  Non-linear 
c  Least  Squares  method  in  IMSL  routine  ZXSSQ  using  function 
c  PUMA_ARM.  The  simulated  poses  are  read  from  the  data  file 
c  PUMA-POS.DAT 

INTEGER  LDFJAC,  MM,  M,  NN,  N,  NSIG,  MAXFN,  I OPT,  IXJAC,  INFER,  IER 
INTEGER  I,  J,  K,  NOBS,  MAXNOBS 

REAL*8  FJAC(LDFJAC,NN),  XJTJ( (NN+1 ) *NN/2 ) 

REAL*8  PARM(4),  F(LDFJAC),  WORK( (5*NN)+(2*MM)+( (NN+1 ) *NN/2 ) ) 

REAL*8  DANGLE,  DLENTH,  TQ,  DQ,  EPS,  DELTA,  SSQ,  S0ERR1 ,  SQERR2 

REAL*8  FIO,  THO,  SIO,  PXO,  PYO,  PZO 

REAL*8  DTI,  DT2 ,  DT3,  DT4,  DT5 

REAL* 8  DD1,  DD2 ,  DD3,  DD4 ,  DD5 

REAL*8  AA1,  AA2 ,  AA3,  AA4,  AA5 

REAL*8  AL1,  AL2,  AL3,  AL4,  AL5 

REAL* 8  BL1,  BL2 ,  BL3,  BL4 ,  BL5 

REAL*8  DF6,  TH6 ,  SI6,  PX6 ,  PY6,  PZ6,  FI6 

REAL*8  TET1 ( MAXNOBS ) ,  TET2 ( MAXNOBS ) ,  TET 3 ( MAXNOBS ) 

REAL*8  TET4 ( MAXNOBS ) ,  TET5 ( MAXNOBS ) ,  TET6 (MAXNOBS) 

REAL*8  TM( 3, 4, MAXNOBS),  SCALE,  X(NN) 

PARAMETER  (LDFJAC=6*360,  MM=LDFJAC,  NN=30) 
PARAMETER  (MAXNOBS=360 ) 

EXTERNAL  PUMA_ARM 

COMMON  /PDATA/  NOBS,  TM,  SCALE, 
&  TET1,  TET2,  TET3,  TET4 ,  TET5,  TET6 

c  Open  data  file  for  storage  of  output.  This  will  be  the  identified 
c  kinematic  parameters. 

OPEN  (8,  NAME= 'RESULT. DAT ' ,  STATUS= ' NEW ' ) 

c  Open  data  file  for  the  simulated  poses.  These  were  generated  in 
c  program  POSE. 

OPEN  (9,  NAME=' PUMA-POS.DAT' ,  STATUS= ' OLD ' ) 
c  Open  data  file  of  nominal  kinematic  parameters. 

OPEN  (10,NAME=' INPUT. DAT',  STATUS= ' OLD ' ) 

c  Read  nominal  kinematic  parameters  from  INPUT.DAT. 

read  (10,*) 

read  (10,*)   fi0,th0,si0,px0,py0,pz0 

read  (10,*)   dtl,ddl,aal,all,bll 

read  (10,*)   dt2,dd2,aa2 ,al2,bl2 
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read    (10,*)  dt3,dd3,aa3,al3,bl3 

read    (10,*)  dt4,dd4,aa4,al4,bl4 

read    (10,*)  dt5,dd5,aa5,al5,t>15 

read    (10,*) 

read    (10,*)  df 6,th6,si6,px6,py6,pz6 

read   (10,*) 

read   (10,*)  nobs, n, dangle, dlenth,magnx,magnl 

CLOSE    (10) 

c  Initialize  the  kinematic  parameter  data  variables  in  the  X  array. 


X 

1)=FI0 

X 

2)=TH0 

X 

3)=SI0 

X 

4 ) =PX0 

X 

5 ) =PY0 

X 

6)=PZ0 

X 

7)=AA1 

X 

8)=AL1 

X 

9)=DT2 

X 

10)=AA2 

X 

11)=AL2 

X 

12)=BL2 

X 

13)=DT3 

X 

14)=DD3 

X 

15)=AA3 

X 

16)=AL3 

X 

17)=DT4 

X 

18)=DD4 

X 

19)=AA4 

X 

20)=AL4 

X 

21)=DT5 

X 

t22)=DD5 

X 

[23)=AA5 

X 

i24)=AL5 

X 

r25)=DF6 

X 

(26)=TH6 

X 

(27)=SI6 

X 

(28)=PX6 

X 

(29)=PY6 

X 

(30)=PZ6 

c  Read  simulated  joint  angles  and  tool  pose  from  PUMA-POS . DAT . 

DO  J  =  1,  NOBS 

READ  (9,*)  TET1(J),  TET2(J),  TET3(J),  TET4(J),  TET5(J),  TET6(J) 


TM(1,1,J),  TM(1,2,J),  TM(1,3,J),  TM(1,4,J) 
TM(2,1,J),  TM(2,2,J),  TM(2,3,J),  TM(2,4,J) 
TM(3,1,J),  TM(3,2,J),  TM(3,3,J),  TM(3,4,J) 


READ  (9,*) 

READ  (9,*) 

READ  (9,*) 

READ  (9,*) 
ENDDO 
CLOSE  (9) 

c  Set  the  scale  factor  for  the  direction  cosines  within  the  T6  matrix. 
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SCALE=100.0 

c  The  following  lines  set  the  parameters  necessary  for  the  operation 
c  of  the  IMSL  routine,  ZXSSQ,  for  identification  of  the  kinematic 
c  parameters . 

c  The  subroutine  which  ZXSSQ  is  calling  is  PUMA_ARM. 

NSIG=4 

EPS=0.0 

DELTA=0 . 0 

MAXFN=1500 

I0PT=1 

IXJAC=LDFJAC 

M=6*NOBS 

CALL  ZXSSQ( PUMA_ARM,M,N,NSIG, EPS, DELTA, MAXFN,IOPT, 
&  PARM,X,SSQ,F,FJAC,IXJAC,XJTJ, WORK, INFER, IER) 

c  Save  identified  kinematic  parameters  to  data  file  RESULT.DAT. 


WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 


(8 

*) 

(8, 

*)     ' 

(8, 

888) 

(8 

*) 

(8, 

*)     ' 

(8, 

888) 

(8 

*) 

(8, 

*)     ' 

(8, 

888) 

(8 

*) 

(8, 

*)     ' 

(8 

888) 

(8 

-*) 

(8 

*)     " 

(8 

888) 

(8 

-*) 

(8 

*)     ' 

(8 

888) 

(8 

,*) 

(8 

*)     ' 

(8 

,889) 

FIO,  THO,  SIO,  PXO,  PYO,  PZO ' 
X(l),  X(2),  X(3),  X(4),  x(5),  x(6) 

DTI,  DD1,  AM,  AL1,  BL1 ' 
0.0,  0.0,  X(7),  X(8) ,  0.0 

DT2,  DD2,  AA2,  AL2 ,  BL2 * 
X(9),  0.0,  X(10),  X(ll),  X(12) 

DT3,  DD3,  AA3,  AL3 ,  BL3 ' 
X(13),  X(14),  X(15),  X(16),  0.0 

DT4 ,  DD4 ,  AA4 ,  AL4 ,  BL4 ' 
X(17),  X(18),  X(19),  X(20),  0.0 

DT5,  DD5,  AA5,  AL5,  BL5 ' 
X(21),  X(22),  X(23),  X(24),  0.0 

DF6,  TH6,  SI6,  PX6,  PY6,  PZ6' 
X(25),  X(26),  X(27),  X(28),  X(29),  X(30) 


888  FORMAT  (  5F12.5  ) 

889  FORMAT  (  6F12.5  ) 

c  Calculate  the  root  mean  square  error  in  identification  of  the 
c  direction  cosine  and  position  parameters. 

TQ  =  DANGLE 
DQ  =  DLENTH 

c  This  calculates  the  error  in  identification  of  the  direction 
c  cosine  parameters. 

SQERR1  = 
&   (FI0+TO^X(l))**2+(TH0+TQ-X(2))**2+(SI0+TQ-X(3))**2 
&   +(AL1+TQ-X(8))**2+(DT2+TQ-X(9))**2+(AL2+TQ-X(11))**2 
&   +(BL2+TO-X(12))**2+(DT3+TQ-X(13))**2+(AL3+TQ-X(16))**2 
&   +(DT4+TQ-X(17) )**2+(AL4+TQ-X(20) )**2 
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&   +(DT5+TQ-X(21) )**2+(AL5+TQ-X(24) )**2 

&   +(DF6+TQ-X(25) ) **2+ (TH6+TQ-X( 26 ) )  **2+( SI6+TQ-X( 27 ) )**2 

SQERR1  =  DSQRT(  SQERR1/16  ) 

c  This  calculates  the  error  in  identification  of  the  position 
c  parameters . 

SQERR2  = 
&   (PX0+DQ-X(4) )**2+(PY0+DQ-X(5) )  **2+(PZ0+DQ-X( 6 ) )**2 
&   +(AA1+DQ-X(7) )**2+(AA2+DQ-X(10) ) **2+(DD3+DQ-X( 14 ) )**2 
&   +(AA3+DQ-X(15) )**2+(DD4+DQ-X(18) ) **2+(AA4+DQ-X( 19) )**2 
&  +(DD5+DQ-X(22) )**2+(AA5+DQ-X(23) )**2 
&   +(PX6+DQ-X(28) )**2+(PY6+DQ-X(29) ) **2+(PZ6+DQ-X(30 ) )**2 

SQERR2  =  DSQRT(  SQERR2/14  ) 

c  Write  the  position  and  orientation  error  to  the  data  file 
c  RESULT.DAT. 

WRITE  (8,*) 

WRITE  (8,*)  'RMS  PARMS  (LENGTH),  RMS  PARMS  (ANGLE)' 

WRITE  (8,*)  SQERR2,  SQERR1 

c  Write  the  position  and  orientation  error  to  the  screen. 

WRITE  (6,*)  'RMS  PARMS  (LENGTH),  RMS  PARMS  (ANGLE)' 
WRITE  (6,*)  SQERR2 , SQERR1 

c  Write  the  ZXSSQ  convergence  criteria  to  the  data  file 
c  RESULT.DAT. 

WRITE  (8,*) 

WRITE  (8,*)  'INFER,  IER,  NOBS,  NSIG' 
WRITE  (8,*)   INFER,  IER,  NOBS,  NSIG 
WRITE  (8,*) 

c  Write  the  ZXSSQ  convergence  criteria  to  the  screen. 

WRITE  (6,*)  'INFER,  IER,  NOBS,  NSIG' 
WRITE  (6,*)   INFER,  IER,  NOBS,  NSIG 

CLOSE  (8) 

END 
c  **SUBROUTINES** 
c  "SUBROUTINE  1** 

SUBROUTINE  PUMA_ARM  (X,  M,  N,  F) 

c  This  subroutine  calculates  the  non-linear  function  for  the  use  of 
c  the  IMSL  routine  ZXSSQ.  It  is  the  forward  kinematic  solution  for 
c  the  PUMA  manipulator. 

INTEGER  M,  N,  II,  JJ 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 

REAL*8  X(N) ,  F(M) 

REAL*8  FIO,  THO,  SIO,  PXO,  PYO,  PZO 

REAL*8  DTI,  DT2 ,  DT3,  DT4,  DT5 
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REAL*8  DD1,  DD2,  DD3,  DD4,  DD5 

REAL*8  AA1,  AA2 ,  AA3 ,  AA4 ,  AA5 

REAL*8  AL1,  AL2 ,  AL3,  AL4,  AL5 

REAL* 8  BL1,  BL2 ,  BL3,  BL4 ,  BL5 

REAL*8  FI6,  TH6,  SI6,  PX6,  PY6,  PZ6,  DF6 

REAL*8  TH1,  TH2 ,  TH3,  TH4,  TH5 

REAL*8  TO (4,4),  Tl(4,4),  T2(4,4),  T3(4,4),  T4(4,4) 

REAL*8  T5(4,4),  T6(4,4),  TRPY(4,4),  TXYZ(4,4) 

REAL*8  TIMAT(4,4),  T(4,4) 

REAL*8  TINV(4,4),  TMJ(4,4),  TDELTA(4,4) 

REAL*8  TET1 ( MAXNOBS ) ,  TET2 ( MAXNOBS ) ,  TET3 ( MAXNOBS ) 

REAL*8  TET4 ( MAXNOBS ) ,  TET5 ( MAXNOBS ) ,  TET 6 ( MAXNOBS ) 

REAL*8  TM( 3, 4, MAXNOBS) ,  SCALE 

PARAMETER  (MAXNOBS=360 ) 

COMMON  /PDATA/  NOBS,  TM,  SCALE, 
&  TET1,  TET2,  TET3,  TET4 ,  TET5,  TET6 

c  Initializing  the  TIMAT  matrix  to  an  identity  matrix. 

DATA  T IMAT / 1 , 0,0, 0,0, 1,0, 0,0, 0,1, 0,0, 0,0,1/ 

c  Assign  kinematic  parameters  to  the  variable  X  array. 

FI0  =  X(l) 

TH0  =  X(2) 

SI0  =  X(3) 

PX0  =  X(4) 

PY0  =  x(5) 

PZ0  =  X(6) 

DTI  -  0.0 
DD1  =0.0 
AA1  =  X(7) 
AL1  =  X(8) 
BL1  =0.0 

DT2  =  X(9) 
DD2  =0.0 
AA2  =  X(10) 
AL2  =  X(ll) 
BL2  =  X(12) 

DT3  =  X(13) 
DD3  =  X(14) 
AA3  =  X(15) 
AL3  =  X(16) 
BL3  =  0.0 

DT4  =  X(17) 
DD4  =  X(18) 
AA4  =  X(19) 
AL4  =  X(20) 
BL4  =0.0 

DT5  =  X(21) 
DD5  =  X(22) 
AA5  =  X(23) 
AL5  =  X(24) 
BL5  =  0.0 
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DF6  =  X(25) 

TH6  =  X(26) 

SI6  =  X(27) 

PX6  =  X(28) 

PY6  =  X(29) 

PZ6  =  X(30) 

c  Loop  through  NOBS  times . 

K  =  0 

DO  J  =  1,  NOBS 

c  Initializing  the  T  matrix  to  an  identity  matrix. 

DO  II  =  1,4 

DO  J J  =  1,4 
T(II,JJ)  =  TIMAT(II,JJ) 

ENDDO 
ENDDO 

c  Calculate  the  actual  manipulator  joint  angles. 

TH1  =  DTI  +  TET1(J) 
TH2  =  DT2  +  TET2(J) 
TH3  =  DT3  +  TET3(J) 
TH4  =  DT4  +  TET4(J) 
TH5  =  DT5  +  TET5(J) 
FI6  =  DF6  +  TET6(J) 

c  Compute  the  T  matrices,  Tl  thru  T6. 

CALL  T3RPY(  FIO,  THO,  SIO,  TRPY  ) 
CALL  T3XYZ(  PXO,  PYO,  PZO,  TXYZ  ) 
CALL  MATMULC(  TO,  TRPY,  TXYZ  ) 

CALL  TRANSFORM  (  AL1 ,  AA1 ,  DD1,  TH1,  BL1,  Tl  ) 

CALL  TRANSFORM  (  AL2 ,  AA2 ,  DD2 ,  TH2 ,  BL2 ,  T2  ) 

CALL  TRANSFORM  (  AL3,  AA3,  DD3,  TH3,  BL3,  T3  ) 

CALL  TRANSFORM  (  AL4 ,  AA4 ,  DD4 ,  TH4 ,  BL4 ,  T4  ) 

CALL  TRANSFORM  (  AL5,  AA5,  DD5,  TH5,  BL5,  T5  ) 

CALL  T3RPY(  FI6,  TH6,  SI6,  TRPY  ) 
CALL  T3XYZ(  PX6,  PY6,  PZ6,  TXYZ  ) 
CALL  MATMULC(T6,  TRPY,  TXYZ  ) 

c  Compute  the  complete  transformation  from  the  base  frame  to 
c  the  tool  frame. 


CALL 

MATMULA 

T, 

TO  ) 

CALL 

MATMULA 

T, 

Tl  ) 

CALL 

MATMULA 

T, 

T2  ) 

CALL 

MATMULA 

T, 

T3  ) 

CALL 

MATMULA 

T, 

T4  ) 

CALL 

MATMULA 

T, 

T5  ) 

CALL 

MATMULA 

T, 

T6  ) 

c  Read  the  simulated  measured  T  matrix  for  this  observation: 

DO  II  =  1,3 
DO  J J  =  1,4 


96 


mj(ii,jj)  =  tm(ii,jj,j) 

END  DO 
ENDDO 

c  Remember  that  row  4  is  0,  0,  0,  1. 

TMJ(4,1)  =  0.0 

TMJ(4,2)  =  0.0 

mJ(4,3)  =  0.0 

TMJ(4,4)  =  1.0 

c  Compute  the  difference  between  the  measured  T  matrix  and  the 
c  T  matrix  generated  using  the  latest  table  of  kinematic  data. 

CALL  MATSUB  (  TDELTA,  TMJ,  T  ) 

c  Calculate  the  function  F  (six  rows  at  a  time) . 

c  First,  position. 

K  =  K  +  1 

F(K)  =  TDELTA(1,4) 

K  =  K  +  1 

F(K)  =  TDELTA(2,4) 

K  =  K  +  1 

F(K)  =  TDELTA(3,4) 

c  Now  orientation. 

K  =  K  +  1 

F(K)  =  ( (TDELTA(3,2)-TDELTA(2,3) )/2.0)  *  SCALE 

K  -  K  +  1 

F(K)  =  ( (TDELTA(1,3)-TDELTA(3,1) )/2.0)  *  SCALE 

K  =  K  +  1 

F(K)  =  ( (TDELTA(2,1)-TDELTA(1,2) )/2.0)  *  SCALE 

c  Ending  the  DO  loop  for  J  counter. 

ENDDO 
c  Write  RMS  error  in  function  F. 

XSSOj=0.0 

DO  II=l,6*NOBS 

XSSQ=XSSQ+F( II ) *F( II ) 
ENDDO 

XER=SQRT(XSSQ) 

c  Write  RMS  error  to  the  screen.  This  allows  the  operator 
c  to  track  how  the  identification  process  is  proceeding. 

WRITE(6,*)  XER 

RETURN 

END 
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APPENDIX  F    PROGRAM  VERIFY 


PROGRAM  VERIFY 

C  This  program  generates  the  six-dof  pose  error  for  the  PUMA  manipulator. 
C  It  contains  the  identified  calibration  parameters  and  the  exact  parameter. 
C  It  uses  a  data  file  of  verification  joint  angle  sets  POSEVER.DAT,  and  the 
C  file  RESULT.DAT  from  the  program  ID6. 

INTEGER  I,  J,  K,  NPOSES,  N,  nob 
REAL*8  DANGLE,  DLENTH 

REAL*8  DT(5) ,dd(5) ,aa(5) ,al(5) ,bl(5) 

REAL*8  eDT(5) ,edd(5) ,eaa(5) ,eal(5) ,ebl(5) 

REAL*8  edf6,  EFI6,  ETH6,  ESI6,  EPX6,  EPY6,  EPZ6 

REAL*8  EDFO,  EFIO,  ETHO,  ESIO,  EPXO,  EP60,  EPZO 

REAL*8  THETA(1000,6) ,  TDELTA(4,4) 

REAL*8  TO (4,4),  Tl(4,4),  T2(4,4),  T3(4,4) 

REAL*8  T4(4,4),  T5(4,4),  T6(4,4),  TRPY(4,4),  TXYZ(4,4) 

REAL*8  TIMAT(4,4),  T(4,4),  et(4,4) 

REAL*8  DFO,  DTO,  DSO,  PXO,  PYO,  PZO 

REAL*8  DTI,  DT2 ,  DT3,  DT4,  DT5 

REAL* 8  DD1,  DD2 ,  DD3,  DD4,  DD5 

REAL*8  AA1,  AA2 ,  AA3 ,  AA4 ,  AA5 

REAL*8  AL1,  AL2 ,  AL3,  AL4 ,  AL5 

REAL* 8  BL1,  BL2 ,  BL3,  BL4,  BL5 

REAL*8  DF6,  DT6,  DS6,  PX6,  PY6,  PZ6 

COMMON  TIMAT,THETA 

EXTERNAL  FKS 
C  Initialize  the  TIMAT  matrix  to  an  I  matrix: 

DATA  TIMAT/ 1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1/ 

C  Open  data  file 

OPEN  ( 9 ,   NAME= ' posever . DAT ' , STATUS= ' OLD ' ) 

OPEN  ( 10 ,  NAME= ' input . DAT ' ,  STATUS= ' OLD ' ) 

OPEN  (11,  NAME= 'result.DAT' ,  STATUS= ' OLD ' ) 

open  (12,  name=' error. dat ' ,access=' append ' ,status=' unknown' ) 

c  Read  input  parameters 

read  (10,*) 

read  (10,*)  df 0,dt0,ds0,px0,py0,pz0 

read  (10,*)  dtl,ddl,aal,all,bll 

read  (10,*)  dt2,dd2,aa2,al2,bl2 

read  (10,*)  dt3,dd3,aa3,al3,bl3 

read  (10,*)  dt4,dd4,aa4,al4,bl4 

read  (10,*)  dt5,dd5,aa5,al5,bl5 

read  (10,*) 

read  (10,*)  df 6,dt6,ds6,px6,py6,pz6 

read  (10,*) 
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read  (10,*)  nobs, n, dangle, dlenth,magnx,magnl 
CLOSE  (10) 

c  Read  in  joint  angle  sets  for  verification  poses 


do  i=l,nobs 

read(9,*)theta(i,l) ,theta(i,2) ,theta(i,3) ,theta(i,4) , 
&  theta(i,5) ,theta(i,6) 

enddo 
close  (9) 

C  Set  exact  link  parameters  for  the  manipulator: 


DT(1)=0.0 

do  i 

=2,5 

dt(i)=dangle 

enddo 

DFO  = 

=  DFO  + 

Dangle 

DTO  = 

=  DTO  + 

DANGLE 

DSO  = 

=  DSO  + 

DANGLE 

PXO  = 

=  PXO  + 

DLENTH 

PYO  = 

=  PYO  + 

DLENTH 

PZO  = 

«  PZO  + 

DLENTH 

al(l) 

=  all 

+  DANGLE 

al(2) 

=  al2 

+  DANGLE 

al(3) 

=  al3 

+  DANGLE 

al(4 

=  al4 

+  DANGLE 

al(5) 

=  al5 

+  DANGLE 

AA(1 

=  aal 

+  DLENTH 

AA(2 

=  aa2 

+  DLENTH 

AA(3 

=  aa3 

+  DLENTH 

AA(4 

=  aa4 

+  DLENTH 

AA(5 

=  aa5 

+  DLENTH 

DD(1 

=  ddl 

!  defined 

DD(2 

=  dd2 

!  defined 

DD(3 

=  dd3 

+  DLENTH 

DD(4 

=  dd4 

+  DLENTH 

DD(5 

=  dd5 

+  DLENTH 

BL(1 

1  =  bll 

!  defined 

BL(2 

=  bl2 

+  DANGLE 

BL(3 

)  =  bl3 

I  defined 

BL(4 

)  =  bl4 

!  defined 

BL(5 

)  =  bl5 

!  defined 

DF6  = 

=  DF6  + 

Dangle 

DT6  = 

=  DT6  + 

DANGLE 

DS6  = 

=  DS6  + 

DANGLE 

PX6  = 

=  PX6  + 

DLENTH 

PY6  = 

=  PY6  + 

DLENTH 

PZ6  ' 

=  PZ6  + 

DLENTH 

c  Read  in  and  set  up  estimated  parameter  table 
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READ  (11,*) 

READ  (11,*) 

READ  (11,*)  EDFO,ETHO/ESIO,EPXO,EPYO,EPZO 

do  i=l,5 

read  (11,*) 

read  (11,*) 

read  (11,*)  edt(i) ,edd(i) ,eaa(i) ,eal(i) ,ebl(i) 
enddo 

read(ll,*) 
read(ll,*) 
read(ll, * )  edf 6,eth6,esi6,epx6,epy6,epz6 

c  Main  loop  through  nobs  joint  angle  sets 

do  k=l,nobs 

call  fks  (k,DF0,DT0,DS0,PX0,PY0,PZ0,dt,al,aa,dd,bl,df6, 
&  DT6,DS6,PX6,PY6,PZ6,t) 

call  fks  (k,EDFO,ETHO,ESIO,EPXO,EPYO,EPZO,edt,eal,eaa,edd, 
&  ebl , edf 6 , eth6 , esi6 , epx6 , epy6 , epz6 , et ) 

c  Compute  the  differential  tool  matrix 

call  matsub  (tdelta,t,et) 

c  Compute  the  pose  errors 

poserr=sqrt ( tdelta (1,4) *  *2+tdelta (2,4) *  *2+tdelta ( 3 , 4 ) *  *  2 ) 
orerr 1= ( tdelta ( 3 , 2 ) -tdelta ( 2 , 3 ) ) /2 
orerr2= ( tdelta( 1,3) -tdelta ( 3 , 1 ) ) /2 
orerr3= ( tdelta (2,1) -tdelta ( 1 , 2 ) ) /2 
orerr=sqrt  ( orerr  1  *  *  2  -t-orerr  2  *  *  2+orerr  3  *  *  2  ) 

c  Update  total  error  counts 

posterr= ( poserr+ ( k-1 ) *pos terr ) /k 
orterr  = ( orerr  + ( k- 1 ) *orterr ) /k 

c  End  of  main  loop 

enddo 

write  (6,*)  'Position  error,  orientation  error' 

write  (6,*)  posterr, orterr 

c      write  (6,*)  'how  many  nobs  in  this  run?' 

c      read  ( 6 , * )  nob 

nob  =  12 

write  (12,*)  'for  nobs  of:', nob 

write  (12,*)  'position  error,  orientation  error' 

write  (12,*)  posterr, orterr 

write  (12,*) 

close  (12) 
end 

Q        ***************************************************************** 

subroutine  fks  (n,DF0,DT0,DS0,PX0,PY0,PZ0,dt,al,aa,dd,bl, 
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&  df 6, th6,si6,px6,py6,pz6,t) 

REAL*8  T0(4,4),  Tl(4,4),  T2(4,4),  T3(4,4) 

REAL*8  T4(4,4),  T5(4,4),  T6(4,4),  TRPY(4,4),  TXYZ (4, 4) 

REAL*8  TIMAT(4,4),  T(4,4),  dt( 5 ) ,al (5) ,aa( 5 ) ,dd(5 ) ,bl ( 5 ) 

REAL* 8  DFO , DTO , DSO , PXO , PYO , PZO , DF6 , TH6 , SI 6 , PX6 , PY6 , PZ6 , FI6 

real*8  theta(1000,6) ,  ang(6) 

common  timat,theta 

C  Initialize  the  T  matrix  to  an  I  matrix: 


DO  J=l,4 
DO  K=l,4 

ENDDO 
ENDDO 


T(J,K)  =  TIMAT(J,K) 


C  Set  up  joint  angles 

do  i=l,5 

ang ( i ) =theta ( n , i ) +dt ( i ) 
enddo 

f i6=df 6+theta ( n, 6 ) 

C  Compute  the  T  matrices,  Tl  thru  T6: 

CALL  T3RPY  (DF0,DT0,DS0,TRPY  ) 
CALL  T3XYZ  (pxO ,pyO,pzO,TXYZ  ) 
CALL  MATMULC  (  TO,  TRPY,  TXYZ  ) 

CALL  TRANSFORM  (al( 1 ) ,aa( 1 ) ,dd( 1 ) ,ang( 1 ) ,bl ( 1 ) ,T1 ) 
CALL  TRANSFORM  (al( 2 ) ,aa(2 ) ,dd(2 ) ,ang(2 ) ,bl(2 ) ,T2 ) 
CALL  TRANSFORM  (al(3 ) ,aa( 3 ) ,dd( 3 ) ,ang( 3 ) ,bl(3 ) ,T3 ) 
CALL  TRANSFORM  ( al ( 4 ) , aa ( 4 ) , dd ( 4 ) , ang ( 4 ) , bl ( 4 ) , T4 ) 
CALL  TRANSFORM  (al (5 ) ,aa(5 ) ,dd(5 ) ,ang( 5 ) ,bl(5) ,T5 ) 

CALL  T3RPY  ( f i6, th6,si6,TRPY  ) 
CALL  T3XYZ  (px6 ,py6 ,pz6 ,TXYZ  ) 
CALL  MATMULC  (  T6 ,  TRPY,  TXYZ  ) 

C  Compute  the  overall  transformation,  T: 


CALL  MATMULA  (  T,  TO 

CALL  MATMULA  (  T,  Tl 

CALL  MATMULA  (  T,  T2 

CALL  MATMULA  (  T,  T3 

CALL  MATMULA  (  T,  T4 

CALL  MATMULA  (  T,  T5 

CALL  MATMULA  (  T,  T6 


return 
end 
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APPENDIX  G    PROGRAM   LINSC 


PROGRAM  linsc 


C  This  program  generates  joint  angles  for  the  Puma  manipulator 

C  arm.   It  presumes  that  the  tool  frame  of  the  manipulator  is 

C  constrained  to  move  in  the  positive  x  direction  only.  The 

C  tool  is  constrained  by  a  sliding  linear  scale.   The  values  along 

C  the  x  direction  are  determined  by  a  random  number  generator.   The 

C  orientation  of  the  tool  is  constrained  as  well .  The  x  direction  of 

C  tool  is  in  the  x  direction  of  the  WCF,  the  y  direction  of  the  tool 

C  is  in  the  z  direction  of  the  WCF  and  the  z  direction  of  the  tool  is 

C  in  the  negative  y  direction  of  the  WCF. 


INTEGER  LDFJAC,  M,  N,  obs ,  nobs 

PARAMETER  (LDFJAC=12,  M=LDFJAC,  N=6  ) 

real*8  fiO,  thO,  siO,  pxO,  pyO,  pzO 

REAL*8  DTI,  DT2 ,  DT3,  DT4 ,  DT5 

REAL* 8  DD1,  DD2 ,  DD3 ,  DD4 ,  DD5 

REAL*8   AA1,    AA2 ,  AA3 ,  AA4 ,  AA5 

REAL*8  AL1,  AL2 ,  AL3,  AL4,  AL5 

REAL*8  BL1,  BL2 ,  BL3,  BL4,  BL5 

REAL*8  DF6,  FI6,  TH6 ,  SI6,  PX6 ,  PY6,  PZ6 

REAL*8  RN1 , RN2 , RN3 , RN4 , RN5 , RN6 
REAL*8  RN7,RN8,RN9,RN10,RN11,RN12 
REAL*8  RN13,RN14,RN15,RN16,RN17,RN18 

INTEGER  infer, ier,iopt,nsig,maxfn 

REAL*8  FJAC(LDFJAC,N) ,  xjt j ( (n+1) *n/2 ) ,  xjac(ldf jac,n) 
REAL*8  parm(4) ,  f(ldfjac),  work( (5*n)+(2*m)+( (n+1) *n/2 ) 
REAL*8  X(N) 
REAL* 8  magnx,magnl 

EXTERNAL  PUMA_ARM 

INTEGER  I,  J,  K,  nou 

REAL*8  TDES(4,4),  T(4,4),  SCALE,  DANGLE,  DLENTH,  NUM 

COMMON  /PDATA/  TDES,  DANGLE,  DLENTH,  T 
COMMON  /KIN/  fiO,  thO,  siO,  pxO,  pyO,  pzO, 

&  DT1,DT2,DT3,DT4,DT5, 

&  AL1,AL2,AL3,AL4,AL5, 

&  AA1,AA2,AA3,AA4,AA5, 

&  DD1 , DD2 , DD3 , DD4 , DD5 , 

&  BL1,BL2,BL3,BL4,BL5, 

&  DF6,TH6,SI6,PX6,PY6,PZ6 

C  Initialize  data  variables 
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obs=0 

C  Open  data  files  for  input 

OPEN  (10,  NAME= ' puma-pos . dat ' ,  STATUS= ' NEW ' ) 
OPEN  ( 9 ,  NAME= ' input . dat ' ,  STATUS= ' OLD ' ) 

C  Read  input  kinematic  data 

read  (9,*) 

read  (9,*)  f i0,th0,si0,px0,py0,pz0 

read  (9,*)  dtl,ddl,aal,all,bll 

read  (9,*)  dt2,dd2,aa2,al2,bl2 

read  (9,*)  dt3,dd3,aa3,al3,bl3 

read  (9,*)  dt4 ,dd4,aa4,al4,bl4 

read  (9,*)  dt5,dd5,aa5,al5,bl5 

read  (9,*) 

read  (9,*)  df 6,th6,si6,px6,py6,pz6 

read  (9,*) 

read  (9,*)  nobs, nou, dangle, dlenth,magnx,magnl 


close  (9) 

C  Adjust  nominal  values 

fi0=fi0+dangle 
th0=th0+dangle 
si0=si0+dangle 
px0=px0+dlenth 
py0=py0+dlenth 
pz0=pz0+dlenth 

dtl=0.0 

dt2=dt2+dangle 

dt3=dt34dangle 

dt4=dt4+dangle 

dt5=dt5+dangle 

all=all+dangle 
al2=al2+d angle 
al3=al3+dangle 
al4=al44dangle 
al5=al5+dangle 

aal=aal+dlenth 
aa2=aa2+dlenth 
aa3=aa3+dlenth 
aa4=aa4+dlenth 
aa5=aa5+dlenth 

ddl=0.0 

dd2=0.0 

dd3=dd3+dlenth 

dd4=dd4+dlenth 

dd5=dd5+dlenth 

bll-bll 

bl2=bl2+dangle 

bl3=bl3 

bl4=bl4 


103 


bl5=bl5 

df6=df6+dangle 

th6=th6+dangle 
c  si6=si6+dangle 
c  px6=px6+dlenth 
c  py6=py6+dlenth 
c      pz6=pz6+dlenth 

C  Get  random  number  seed 

write  (6,*)  'Type  in  a  6-digit  random  number  seed' 
read  (5,*)  iseed 

C  Start  of  main  loop 

1010       obs=obs+l 

C  Set  joint  angles  to  zero 

do  i=l,n 
x(i)=0.0 
enddo 

C  Get  random  bar  angles 

1000   call  random  (iseed, num) 

num=num*900 . 0 

C  Establish  desired  tool  pose 

do  ii=l,4 
do  jj=l,4 

TDES(ii, j j )=0.0 
enddo 
enddo 

TDES (1,4)=  num 
TDES (4,4)=   1.0 
TDES (1,1)=   1.0 
TDES (3,2)=   1.0 
TDES (2,3)=  -1.0 

C  Call  IMSL  ZXSSQ  for  inverse  kinematic  solution 

nsig=4 
eps=0.0 
delta=0 . 0 
maxfn=500 
iopt=l 
ixjac=ldf jac 

CALL  ZXSSQ (puma_arm,m,n,nsig,eps, delta, maxfn,iopt,parm,x, 
&  ssq , f , x j  ac , ix j  ac , x j t j , work , infer , ier ) 

C  Check  for  singularities 

if  (ssq  .gt.  0.00001)  goto  1000 

C  Print  results  to  2  decimal  places 
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write(6,*)   obs,ssq 
C  Generate  the  random  noise 


CALL 

RANDOM 

ISEED,RN1) 

CALL 

RANDOM 

ISEED,RN2) 

CALL 

RANDOM 

ISEED,RN3) 

CALL 

RANDOM 

ISEED,RN4) 

CALL 

RANDOM 

ISEED,RN5) 

CALL 

RANDOM 

ISEED,RN6) 

CALL 

RANDOM 

ISEED,RN7) 

CALL 

RANDOM 

ISEED,RN8) 

CALL 

RANDOM 

ISEED,RN9) 

CALL 

RANDOM 

ISEED,RN10) 

CALL 

RANDOM 

ISEED,RN11) 

CALL 

RANDOM 

ISEED,RN12) 

CALL 

RANDOM 

ISEED,RN13) 

CALL 

RANDOM 

ISEED,RN14) 

CALL 

RANDOM 

ISEED,RN15) 

CALL 

RANDOM 

ISEED,RN16) 

CALL 

RANDOM 

ISEED,RN17) 

CALL 

RANDOM 

ISEED,RN18) 

RN1  = 

MAGNX 

*  (2.0  * 

RN1  - 

1.0) 

RN2  = 

MAGNX 

*  (2.0  * 

RN2  - 

1.0) 

RN3  ■ 

MAGNX 

*  (2.0  * 

RN3  - 

1.0) 

RN4  = 

MAGN1 

*  (2.0  * 

RN4  - 

1.0) 

RN5  = 

MAGN1 

*  (2.0  * 

RN5  - 

1.0) 

RN6  = 

MAGN1 

*  (2.0  * 

RN6  - 

1.0) 

RN7  = 

MAGN1 

*  (2.0  * 

RN7  - 

1.0) 

RN8  = 

MAGN1 

*  (2.0  * 

RN8  - 

1.0) 

RN9  = 

MAGN1 

*  (2.0  * 

RN9  - 

1.0) 

RN10 

=  MAGN1 

*  (2.0  * 

RN10 

-  1.0) 

RN11 

=  MAGN1 

*  (2.0  * 

RN11 

-  1.0) 

RN12 

=  MAGN1 

*  (2.0  * 

RN12 

-  1.0) 

RN13 

=  MAGN1 

*  (2.0  * 

RN13 

-  1.0) 

RN14 

=  MAGN1 

*  (2.0  * 

RN14 

-  1.0) 

RN15 

=  MAGN1 

*  (2.0  * 

RN15 

-  1.0) 

RN16 

=  MAGN1 

*  (2.0  * 

RN16 

-  1.0) 

RN17 

=  MAGN1 

*  (2.0  * 

RN17 

-  1.0) 

RN18 

=  MAGN1 

*  (2.0  * 

RN18 

-  1.0) 

T(l,4) 

=  T(l,< 

1)  +  RN1 

T(2,4) 

=  T(2,- 

i)    +  RN2 

T(3,4) 

=  T(3,- 

i)    +  RN3 

T(l,l) 

-  T(l, 

L)  +  RN4 

T(l,2) 

=  T(l, 

2)  +  RN5 

T(l,3) 

=  T(l, 

3)  +  RN6 

T(2,l) 

-  T(2, 

L)  +  RN7 

T(2,2) 

=  T(2, 

2)  +  RN8 

T(2,3) 

=  T(2, 

3)  +  RN9 

T(3,l) 

=  T(3, 

1)  +  RN10 

T(3,2) 

-  T(3, 

2)  +  RN11 

T(3,3) 

=  T(3, 

3)  +  RN12 

X(l)    =  X(l)    +  RN13 
X(2)    =  X(2)    +  RN14 
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X(3)  =  X(3)  +  RN15 
X(4)  =  X(4)  +  RN16 
X(5)  =  X(5)  +  RN17 
X(6)  =  X(6)  +  RN18 

write  (10,*)  X(1),X(2),X(3),X(4),X(5),X(6) 
write  (10,*) 

write  (10,*)  T(1,1),T(1,2),T(1,3),T(1,4) 

write  (10,*)  T(2,1),T(2,2),T(2,3),T(2,4) 

write  (10,*)  T(3,l ) ,T(3,2 ) ,T(3,3 ) ,T(3,4 ) 
write  (10,*) 

991  FORMAT  (  6F12.6  ) 

992  FORMAT  (  3F16.10,  F12.5  ) 

C  Continue  for  other  bar  angles 

if  (obs  .It.  nobs)  go  to  1010 
CLOSE  (10) 

END 

Q     **************************************************************** 

SUBROUTINE  PUMA_ARM  (X,M,N,F) 

C  This  subroutine  calculates  the  non-linear  function  for  the  use  of 
C  the  IMSL  routine  ZXSSQ.  It  is  the  forward  kinematic  solution  for 
C  the  PUMA  manipulator. 

INTEGER  M,  N 
REAL*8  X(N) ,  F(M) 

INTEGER  II,  J J 

real*8  fiO,  thO,  siO,  pxO,  pyO,  pzO 

REAL*8  DTI,  DT2 ,  DT3,  DT4,  DT5 

REAL* 8  DD1,  DD2 ,  DD3,  DD4,  DD5 

REAL*8  AA1,  AA2 ,  AA3,  AA4 ,  AA5 

REAL*8  AL1,  AL2 ,  AL3,  AL4,  AL5 

REAL*8  BL1,  BL2 ,  BL3,  BL4 ,  BL5 

REAL*8  DF6,  FI6,  TH6,  SI6,  PX6,  PY6,  PZ6 

REAL*8  TH1,  TH2 ,  TH3 ,  TH4 ,  TH5 

REAL*8  TO (4,4),  Tl(4,4),  12(4,4),  T3(4,4),  T4(4,4) 
REAL*8  T5(4,4),  T6(4,4),  trpy(4,4),  txyz(4,4) 
REAL*8  TIMAT(4,4),  T(4,4),  td(4,4) 

INTEGER  I,  J,  K 

REAL*8  TDES(4,4),  DANGLE,  DLENTH, scale 

COMMON  /PDATA/  TDES,  DANGLE,  DLENTH,  T 

COMMON  /KIN/  fi0,th0,si0,px0,py0,pz0, 
&  DT1,DT2,DT3,DT4,DT5, 

&  AL1,AL2,AL3,AL4,AL5, 

&  AA1,AA2,AA3,AA4,AA5, 

&  DD1,DD2,DD3,DD4,DD5, 

&  BL1,BL2,BL3,BL4,BL5, 

&  DF6,TH6,SI6,PX6,PY6,PZ6 

C  Initialize  the  TIMAT  matrix  to  an  I  matrix: 
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DATA  TIMAT/1, 0,0, 0,0, 1,0, 0,0, 0,1, 0,0, 0,0,1/ 

scale=100.0 

C  Initialize  the  T  matrix  to  an  I  matrix 

DO  II  =  1,4 
DO  J J  =  1,4 

T(II,JJ)  =  TIMAT(II,JJ) 
ENDDO 
ENDDO 

C  Manipulator  joint  angles 


TH1  =  DTI 

+ 

X(l) 

TH2  =  DT2 

+ 

X(2) 

TH3  =  DT3 

+ 

X(3) 

TH4  =  DT4 

+ 

X(4) 

TH5  =  DT5 

+ 

X(5) 

FI6  =  DF6 

+ 

X(6) 

C  Compute  the  T  matrices,  Tl  thru  T6: 

call  t3rpy  (fi0,th0,siO,  trpy) 
call  t3xyz  (px0,py0,pz0,  txyz) 
call  matmulc  (to, trpy, txyz) 

CALL  TRANSFORM  (  AL1 ,  AA1 ,  DD1 ,  TH1,  BL1 ,  Tl  ) 

CALL  TRANSFORM  (  AL2 ,  AA2 ,  DD2 ,  TH2 ,  BL2 ,  T2  ) 

CALL  TRANSFORM  (  AL3 ,  AA3,  DD3,  TH3,  BL3,  T3  ) 

CALL  TRANSFORM  (  AL4,  AA4 ,  DD4,  TH4,  BL4 ,  T4  ) 

CALL  TRANSFORM  (  AL5 ,  AA5,  DD5,  TH5,  BL5,  T5  ) 

CALL  t3rpy  (  fi6,  th6,  si6,  trpy  ) 
CALL  T3XYZ  (  PX6,  PY6,  PZ6,  txyz  ) 
CALL  matmulc  (  t6,  trpy,  txyz  ) 

C  Compute  the  overall  transformation,  T: 

CALL  MATMULA  (  T,  TO 

CALL  MATMULA  (  T,  Tl 

CALL  MATMULA  (  T,  T2 

CALL  MATMULA  (  T,  T3 

CALL  MATMULA  (  T,  T4 

CALL  MATMULA  (  T,  T5 

CALL  MATMULA  (  T,  T6 

call  matsub(td,tdes,t) 

C  Calculate  the  function  F 

f(l)=td(l,4) 
f(2)=td(2,4) 
f(3)=td(3,4) 
f (4)=td(l,l)*scale 
f (5)=td(l,2)*scale 
f (6)=td(l,3)*scale 
f (7)=td(2,l)*scale 
f (8)=td(2,2)*scale 
f (9)=td(2,3)*scale 
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f (10)=td(3,l)*scale 
f (ll)=td(3,2)*scale 
f (12)=td(3,3)*scale 

sum  =0.0 
do  i-1,12 

xssq=sum+f (i) 
enddo 
C      write  (6,*)  xssq 

RETURN 

END 

Q     *********************************************************** 

SUBROUTINE  RANDOM  (x,z) 

C  This  subroutine  generates  random  numbers  in  the  range  0-1 
C  using  a  supplied  seed  x,  the  returned  random  number  being  z. 

REAL  FM,  FX,  Z 
INTEGER  A,  X,  I,  M 
DATA  1/1/ 

IF  (  I  .EQ.  0  )  GO  TO  1000 

1=0 

M=  2  **  20 

FM=  M 

A=  2**10  +  3 

1000   X=  MOD(  A*X  ,M) 
FX=  X 

Z=  FX/  FM 

RETURN 
END 
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APPENDIX  H    PROGRAM  ACDAT 


program  acdat 

C  This  program  is  the  data  acquisition  program  for  the  linear 

C  slide  version  of  the  puma  manipulator  calibration  experiments. 

C  It  requires  that  the  value  for  the  distance  down  the  linear 

C  slide  and  the  six  joint  angles  of  the  puma  when  it  is  in  that 

C  configuration.  The  pose  data  is  saved  in  the  file  "slide-pos.dat." 

C  That  file  is  set  up  in  the  append  mode. 

real*8  x,  thl,  th2,  th3,  th4,  th5,  th6 

real*8  tm(4,4) 

open  (10,  name= 'slide-pos .dat' ,access= 'append' ,status=' unknown' 

C  Inputting  the  distance  down  the  linear  slide. 

100     write  (6,*)  'input  the  value  for  x' 
read  ( 6 ,  *  )  x 

C  Inputting  the  six  joint  angles. 

write  (6,*)  'input  the  6  joint  angles' 
read  (6,*)  thl,  th2,  th3,  th4,  th5,  th6 

C  This  will  set  the  T6  matrix  for  the  value  of  x  measured. 

do  ii=l,4 
do  jj-1,4 

tm(ii, j j)=0.0 
enddo 
enddo 

tm(l,4)=  x 

tm(4,4)=  1.0 

tm(l,l)=  1.0 

tm(3,2)=  1.0 

tm(2,3)=  -1.0 

C  Writing  the  pose  data  to  "slide-pos.dat" 

write  (10,991)  thl,  th2 ,  th3,  th4,  th5,  th6 

write  (10,*) 

write  (10,992)  tm(l,l) ,tm(l,2 ) ,tm(l,3) ,tm(l,4) 

write  (10,992)  tm(2,l) ,tm(2,2 ) ,tm(2,3) ,tm(2,4) 

write  (10,992)  tm(3, 1) ,tm(3,2 ) ,tm(3,3) ,tm(3,4) 

write  (10,992)  tm(4,l)  ,tm(4,2) ,tm(4,3) ,tm(4,4) 

write  (10,*) 

write  (6,*) 

write  (6,*)  'data  saved' 

write  (6,*) 
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write  (6,*)  'do  you  wish  to  make  another  observation' 

write  ( 6 , * )  ' enter  a  1  for  yes  or  a  0  for  no ' 

write  (6,*) 

read  ( 6 , * )  ans 

if  (ans  .eq.  1)  go  to  100 

991  FORMAT  (  6F12.6  ) 

992  FORMAT  (  3F16.10,  F12.5  ) 

close (10) 

stop 

end 
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APPENDIX   I     PROGRAM   ID6  LINSC 


PROGRAM  ID6_LINSC 

C  Robot  Identification  using  the  Non-linear  Least  Squares  method. 
C  Simulation  data  is  read  for  the  PUMA  manipulator  from 
C  the  data  file  PUMA-POS.DAT 

C  Change  parameter  LDFJAC  to  change  the  number  of  observations, 
C  set  LDFJAC  =  6  *  Number  of  observations 

INTEGER  LDFJAC,  MM,  M,  NN,  N,  NSIG,  MAXFN,  IOPT,  IXJAC,  INFER,  IER 
PARAMETER  (LDFJAC=12*42 ,  MM=LDFJAC,  NN=26) 

REAL*8  FJAC(LDFJAC,NN),  XJTJ( (NN+1 ) *NN/2 ) 

REAL*8  PARM(4),  F(LDFJAC),  WORK(  (  5*NN  )  +  (  2*MM)  +  (  (NN+1 )  *NN/'2  )  ) 
REAL*8  X(NN) 
EXTERNAL  PUMA_ARM 

REAL*8  DANGLE,  DLENTH,  TQ,  DQ,  EPS,  DELTA,  SSQ 
REAL* 8  SQERR1,  SQERR2 

real*8  f iO,thO,siO,pxO,pyO,pzO 
REAL*8  DTI,  DT2 ,  DT3,  DT4 ,  DT5 
REAL* 8  DD1,  DD2 ,  DD3,  DD4 ,  DD5 
REAL*8  AA1,  AA2 ,  AA3,  AA4,  AA5 
REAL*8  AL1,  AL2 ,  AL3,  AL4 ,  AL5 
REAL*8  BL1,  BL2 ,  BL3 ,  BL4 ,  BL5 
REAL*8  DF6,  TH6,  SI6,  PX6,  PY6 ,  PZ6,  FI6 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 
PARAMETER  (MAXNOBS=360 ) 

REAL*8  TET1 (MAXNOBS) ,  TET2 ( MAXNOBS ) ,  TET3 ( MAXNOBS ) 
REAL*8  TET4 ( MAXNOBS ) ,  TET5 ( MAXNOBS ) ,  TET6 (MAXNOBS ) 
REAL*8  TM( 3, 4, MAXNOBS),  SCALE 
COMMON  /PDATA/  NOBS,  TM,  SCALE, 
&  TET1,  TET2,  TET3,  TET4 ,  TET5,  TET6 

C  Open  data  files  for  inputs  and  results 

OPEN  (8,  NAME=' RESULT. DAT ' ,  STATUS= ' NEW ' ) 
OPEN  (9,  NAME=' PUMA- POS. DAT ' ,  STATUS= ' OLD ' ) 
OPEN  ( 10,NAME=' INPUT.DAT ' ,  STATUS= ' OLD ' ) 

c  Read  input  parameters 

read  (10,*) 

read  (10,*)  fi0,th0,si0,px0,py0,pz0 

read  (10,*)  dtl,ddl,aal,all,bll 

read  (10,*)  dt2,dd2,aa2,al2,bl2 

read  (10,*)  dt3,dd3,aa3,al3,bl3 

read  (10,*)  dt4,dd4,aa4,al4,bl4 

read  (10,*)  dt5,dd5,aa5,al5,bl5 

read  (10,*) 

read  (10,*)  df 6,th6,si6,px6,py6,pz6 
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read  (10,*) 

read  (10,*)  nobs, n, dang le,dlenth,magnx,magnl 

CLOSE  (10) 

write  ( 6 , * )  ' enter  nobs ' 
read  ( 6 , * )  nobs 

C  Initialize  data  variables 


X(l)=fi0 

X(2)=th0 

X(3)=si0 

X(4)=px0 

x(5)«py0 

x(6)=pz0 

X ( 7 ) -AA1 

X(8)=AL1 

X(9)=DT2 

X(10)=AA2 

X(11)=AL2 

X(12)=BL2 

X(13)=DT3 

X(14)=DD3 

X(15)=AA3 

X(16)=AL3 

X(17)=DT4 

X(18)=DD4 

X(19)=AA4 

X(20)=AL4 

X(21)=DT5 

X(22)=DD5 

X(23)=AA5 

X(24)=AL5 

x(25)=df6 

x(26)=th6 

c 

x(27)=si6 

c 

x(28)=px6 

c 

x(29)=py6 

c 

x(30)=pz6 

C  Read  simulated  joint  data  and  tool  pose 

DO  J  =  1,  NOBS 

READ  (9,*)  TET1(J),  TET2(J),  TET3(J),  TET4(J),  TET5(J),  TET6(J) 
read  (9,*) 

READ  (9,*)  TM(1,1,J),  TM(1,2,J),  TM(1,3,J),  TM(1,4,J) 
READ  (9,*)  TM(2,1,J),  TM(2,2,J),  TM(2,3,J),  TM(2,4,J) 
READ  (9,*)  TM(3,1,J),  TM(3,2,J),  TM(3,3,J),  TM(3,4,J) 
READ  (9,*) 
C         READ  (9,*) 

ENDDO 

CLOSE  (9) 

C  Initialize  scale  for  the  angular  rows  of  the  Jacobian 
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SCALE=100.0 

C  Call  IMSL  routine  for  non-linear  identification 

NSIG=3 

EPS=0 . 0 

DELTA=0 . 0 

MAXFN=1500 

I0PT=1 

IXJAC=LDFJAC 

M=12*N0BS 

CALL  ZXSSQ(PUMA_ARM,M,N,NSIG, EPS, DELTA, MAXFN,IOPT, 
&  PARM,X,SSQ,F,FJAC, IXJAC, XJTJ, WORK, INFER, IER) 

C  Save  results  to  data  file 


WRITE 
WRITE 

WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 


(8 

-*) 

(8, 

*)     ' 

(8 

888) 

(8 

.*) 

(8, 

*)     ' 

(8 

888) 

(8 

-*) 

(8 

*)     ' 

(8 

888) 

(8 

-*) 

(8 

*)     ' 

(8 

888) 

(8 

-*) 

(8 

*)     ' 

(8 

888) 

(8 

r*) 

(8 

*)      ' 

(8 

888) 

(8 

,*) 

(8 

*)     ' 

(8 

889) 

fiO,  thO,  siO,  pxO,  pyO,  pzO ' 

X(l),  X(2),  X(3),  X(4),  x(5),  x(6) 

DTI,  DD1,  AA1,  AL1,  BL1 ' 
0.0,  0.0,  X(7) ,  X(8),  0.0 

DT2,  DD2,  AA2,  AL2,  BL2 ' 
X(9),  0.0,  X(10),  X(ll),  X(12) 

DT3,  DD3,  AA3,  AL3,  BL3 ' 
X(13),  X(14),  X(15),  X(16),  0.0 

DT4,  DD4,  AA4,  AL4 ,  BL4 ' 
X(17),  X(18),  X(19),  X(20),  0.0 

DT5,  DD5,  AA5,  AL5 ,  BL5 ' 
X(21),  X(22),  X(23),  X(24),  0.0 

DF6,  TH6,  SI6,  PX6,  PY6,  PZ6 ' 
x(25),x(26),  si6,  px6,  py6,  pz6 


888  FORMAT  (  5F12.5  ) 

889  FORMAT  (  6F12.5  ) 

C  Calculate  root  mean  square  error  in  identification 


TQ 
DO. 


DANGLE 
DLENTH 


C  Error  in  identification  ( angular  parameters ) 

SQERR1  = 
&   (FI0+T<2-X(1) )**2  +(TH0+TQ-X(2) )**2  +(SI0+TQ-X(3) ) **2 
&   +(DT3+TQ-X(13) )**2  +(DT4+TQ-X(17 ) ) **2  +  (DT5+T'Q-X(21 )  )  **2 
&   +(AL1+TQ-X(8) )**2  +(AL2+TQ-X(11) )**2 

&   +(AL3+TQ-X(16) )**2  +(AL4+TQ-X( 20 ) ) **2  +(AL5+TQ-X(24 ) ) **2 
&   +(BL2+TQ-X(12) )**2  +(DT2+TQ-X( 9) )**2 
&  +(df6+tq-x(25) )**2  +(th6+tq-x(26) )**2 

SQERR1  =  DSQRT(  SQERR1/15  ) 

C  Error  in  identification  ( length  parameters ) 
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SQERR2  = 

&   (PX0+DQ-X(4)  )**2  +(AAl+0.0+DQ-X(7) )**2  +(AA2+DQ-X( 10 ) ) **2 
&   +(AA3+DQ-X(15) )**2  +(AA4+DQ-X( 19 ) ) **2  +(AA5+DQ-X(23 ) ) **2 
&   +(PY0+DQ-X(5) )**2  +(PZ0+DQ-X(6) )**2 

&   +(DD3+DQ-X(14) )**2  +  (DD4+D<2-X(  18  )  )  **2  +(DD5+DQ-X(22 ) ) **2 
SOERR2  =  DSQRT(  SQERR2/11  ) 

WRITE  (8,*) 

WRITE  (8,*)  'RMS  PARMS  (LENGTH),  RMS  PARMS  (ANGLE)" 

WRITE  (8,*)  SQERR2,  SQERR1 

WRITE  (6,*)  'RMS  PARMS  (LENGTH),  RMS  PARMS  (ANGLE)' 

WRITE  (6,*)  SQERR2 , SQERR1 


WRITE  (8,*) 

WRITE  (8,*)  'INFER,  IER,NOBS,NSIG' 

WRITE  (8,*)  INFER,  IER,NOBS,NSIG 

WRITE  (6,*)  'INFER,  IER,NOBS,NSIG ' 

WRITE  (6,*)  INFER,  IER,NOBS,NSIG 

WRITE  (8,*) 

CLOSE  (8) 

END 

Q  *************************************************************** 

SUBROUTINE  PUMA_ARM  (X,  M,  N,  F) 

C  This  subroutine  calculates  the  non-linear  function  for  the  use  of 
C  the  IMSL  routine  DUNLSF.  It  is  the  forward  kinematic  solution  for 
C  the  PUMA  manipulator. 

INTEGER  M,  N 
REAL*8  X(N) ,  F(M) 

INTEGER  II,  JJ 

real*8  fi0,th0,si0,px0,py0,pz0 
REAL*8  DTI,  DT2 ,  DT3 ,  DT4 ,  DT5 
REAL* 8  DD1,  DD2 ,  DD3,  DD4,  DD5 
REAL*8  AA1,  AA2 ,  AA3 ,  AA4,  AA5 
REAL*8  AL1,  AL2 ,  AL3,  AL4,  AL5 
REAL*8  BL1,  BL2 ,  BL3,  BL4,  BL5 
REAL*8  FI6,  TH6,  SI6,  PX6,  PY6,  PZ6,  DF6 

REAL*8  TH1,  TH2 ,  TH3,  TH4 ,  TH5 

REAL*8  TO (4,4),  Tl(4,4),  T2(4,4),  T3(4,4),  T4(4,4) 

REAL*8  T5(4,4),  T6(4,4),  TRPY(4,4),  TXYZ(4,4) 

REAL*8  TIMAT(4,4),  T(4,4) 

REAL*8  TINV(4,4),  TMJ(4,4),  TDELTA(4,4) 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 
PARAMETER  (MAXNOBS=360 ) 

REAL*8  TET1 ( MAXNOBS ) ,  TET2 ( MAXNOBS ) ,  TET 3 ( MAXNOBS ) 
REAL*8  TET4( MAXNOBS),  TET5 ( MAXNOBS ) ,  TET6 ( MAXNOBS ) 
REAL*8  TM( 3, 4, MAXNOBS) ,  SCALE 
COMMON  /PDATA/  NOBS,  TM,  SCALE, 
&  TET1,  TET2,  TET3,  TET4 ,  TET5 ,  TET6 
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C  Initialize  the  TIMAT  matrix  to  an  I  matrix: 

DATA  TIMAT/ 1 , 0,0,0,0,1,0,0,0,0,1,0,0,0,0,1/ 

C  Set  parameters  for  the  manipulator: 

fiO  =  X(l) 
thO  =  X(2) 
siO  =  X(3) 
pxO  =  X(4) 
pyO  =  x(5) 
pzO  =  x(6) 

DTI  =  0.0 

DD1  =0.0 

AA1  =  X(7) 

AL1  =  X(8) 

BL1  =  0.0 

DT2  =  X(9) 
DD2  =0.0 
AA2  =  X(10) 
AL2  =  X(ll) 
BL2  =  X(12) 

DT3  =  X(13) 

DD3  =  X(14) 

AA3  =  X(15) 

AL3  =  X(16) 

BL3  =0.0 

DT4  =  X(17) 
DD4  =  X(18) 
AA4  =  X(19) 
AL4  =  X(20) 
BL4  =  0.0 

DT5  =  X(21) 

DD5  =  X(22) 
AA5  =  X(23) 
AL5  =  X(24) 
BL5  =0.0 

DF6  =  x(25) 

TH6  =  X(26) 

SI6  =  0.0   lx(27) 

px6  =  0.0   lx(28) 

py6  =  0.0   tx(29) 

pz6  =  55.9  !x(30) 

C  Loop  NOBS  times 

K  =  0 

DO  J  =  1,  NOBS 

C  Initialize  the  T  matrix  to  an  I  matrix 

DO  II  =  1,3 
DO  J J  =  1,4 

T(II,JJ)    =  TIMAT(II,JJ) 
ENDDO 
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ENDDO 


C  Manipulator  joint  angles 


TH1  =  DTI 
TH2  =  DT2 
TH3  =  DT3 
TH4  =  DT4 
TH5  =  DT5 
FI6  =  DF6 


TET1( J) 
TET2(J) 
TET3(J) 
TET4( J) 
TET5(J) 
TET6(J) 


C  Compute  the  T  matrices,  Tl  thru  T6: 

call  t3rpy(fi0,th0,si0,  trpy) 
call  t3xyz(pxO,pyO,pzO,  txyz) 
call  matmulc(tO, trpy, txyz) 

CALL  TRANSFORM  (  AL1,  AA1 ,  DD1,  TH1,  BL1 ,  Tl  ) 

CALL  TRANSFORM  (  AL2 ,  AA2 ,  DD2 ,  TH2 ,  BL2,  T2  ) 

CALL  TRANSFORM  (  AL3,  AA3,  DD3,  TH3,  BL3,  T3  ) 

CALL  TRANSFORM  (  AL4 ,  AA4,  DD4 ,  TH4,  BL4,  T4  ) 

CALL  TRANSFORM  (  AL5 ,  AA5,  DD5 ,  TH5 ,  BL5,  T5  ) 

CALL  T3RPY  (  FI6,  TH6,  SI6,  TRPY  ) 
CALL  T3XYZ  (  PX6 ,  PY6 ,  PZ6,  TXYZ  ) 
CALL  MATMULC  (T6,  TRPY,  TXYZ  ) 

C  Compute  the  overall  transformation,  T: 


CALL 

MATMULA 

T, 

TO  ) 

CALL 

MATMULA 

T, 

Tl  ) 

CALL 

MATMULA 

T, 

T2  ) 

CALL 

MATMULA 

T, 

T3  ) 

CALL 

MATMULA 

T, 

T4  ) 

CALL 

MATMULA 

T, 

T5  ) 

CALL 

MATMULA 

T, 

T6  ) 

C  Get  the  "T-measured"  matrix  for  this  observation: 

DO  II  =  1,4 

DO  J J  =  1,4 

TMJ(II,JJ)  =  tm(ii,jj,j) 

ENDDO 
ENDDO 

c       tmj(3,2)=-1.0 
c       tmj(2,3)=1.0 

TMJ(4,1)  =  0.0 

TMJ(4,2)  =  0.0 

TMJ(4,3)  =  0.0 

TMJ(4,4)  =  1.0 

C  Compute  the  elements  of  the  "delta-Tn" : 

CALL  MATSUB  (  TDELTA,  TMJ,  T  ) 

C  Calculate  the  function  F  (six  rows  at  a  time) 

f (k+l)=tdelta(l,4) 
f (k+2)=tdelta(2,4) 
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f (k+3)=tdelta(3,4) 

f ( k+4 ) =tdelta (1,1) *scale 
f (k+5)=t delta (1,2) *scale 
f (k+6 )=tdelta( 1 , 3 ) *scale 
f ( k+7 ) =tdelta( 2 , 1 ) *scale 
f ( k+8 ) "tdelta (2,2) *scale 
f ( k+9 ) =tdelta (2,3) *scale 
f (k+10)=tdelta(3,l)*scale 
f(k+ll)=tdelta(3,2)*scale 
f (k+12)=tdelta(3,3)*scale 

k=k+12 
C  End  the  do-loop  for  counter  J 

ENDDO 

C  Write  RMS  error  in  F 

XSSQ=0 . 0 

DO   11=1,1 2 *NOBS 

XSSOj=XSSO+F ( II )  *F(  II ) 
ENDDO 

XER=SQRT(XSSQ) 
WRITE (6,*)    XER 

RETURN 
END 
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